diff --git a/conf/airframes/ardrone2_opticflow_hover.xml b/conf/airframes/ardrone2_opticflow_hover.xml new file mode 100644 index 0000000000..fa1f9810e0 --- /dev/null +++ b/conf/airframes/ardrone2_opticflow_hover.xml @@ -0,0 +1,230 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + +
+ + + + + + + + + +
+ + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + +
+ + + +
+ + + + + + +
+ +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + + + + + +
+ +
+ + + +
+ +
+ + + + +
+ +
+ + + + + +
+ diff --git a/conf/conf_example.xml b/conf/conf_example.xml index ab36044051..523d5e8e38 100644 --- a/conf/conf_example.xml +++ b/conf/conf_example.xml @@ -7,8 +7,8 @@ telemetry="telemetry/default_rotorcraft.xml" flight_plan="flight_plans/dummy.xml" settings="settings/rotorcraft_basic.xml" - gui_color="white" settings_modules="" + gui_color="white" /> + + - diff --git a/conf/firmwares/subsystems/rotorcraft/navigation.makefile b/conf/firmwares/subsystems/rotorcraft/navigation.makefile index 2890d3b271..49e29abc30 100644 --- a/conf/firmwares/subsystems/rotorcraft/navigation.makefile +++ b/conf/firmwares/subsystems/rotorcraft/navigation.makefile @@ -2,4 +2,5 @@ $(TARGET).CFLAGS += -DUSE_NAVIGATION $(TARGET).srcs += $(SRC_FIRMWARE)/navigation.c +$(TARGET).srcs += subsystems/navigation/waypoints.c $(TARGET).srcs += subsystems/navigation/common_flight_plan.c diff --git a/conf/firmwares/subsystems/fixedwing/current_sensor.makefile b/conf/firmwares/subsystems/shared/current_sensor.makefile similarity index 100% rename from conf/firmwares/subsystems/fixedwing/current_sensor.makefile rename to conf/firmwares/subsystems/shared/current_sensor.makefile diff --git a/conf/firmwares/subsystems/shared/sdlog.makefile b/conf/firmwares/subsystems/shared/sdlog.makefile index 071141beba..62ee4f683e 100644 --- a/conf/firmwares/subsystems/shared/sdlog.makefile +++ b/conf/firmwares/subsystems/shared/sdlog.makefile @@ -1,6 +1,6 @@ # Hey Emacs, this is a -*- makefile -*- -sdlog_CFLAGS = -DDOWNLINK +sdlog_CFLAGS = -DDOWNLINK -DUSE_PPRZLOG sdlog_srcs = subsystems/datalink/downlink.c subsystems/datalink/pprzlog_transport.c ap.CFLAGS += $(sdlog_CFLAGS) diff --git a/conf/joystick/hobbyking_usb_rc_6ch_rc_tx_joy_mode_2.xml b/conf/joystick/hobbyking_usb_rc_6ch_rc_tx_joy_mode_2.xml new file mode 100644 index 0000000000..b778a15957 --- /dev/null +++ b/conf/joystick/hobbyking_usb_rc_6ch_rc_tx_joy_mode_2.xml @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/conf/messages.xml b/conf/messages.xml index 3613efc8d1..42c451aba9 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1972,8 +1972,30 @@ - - + + + + + + + + + + + + + + + + + + + + + + + + @@ -1997,7 +2019,7 @@ - + diff --git a/conf/modules/ctrl_module_demo.xml b/conf/modules/ctrl_module_demo.xml new file mode 100644 index 0000000000..9a32233005 --- /dev/null +++ b/conf/modules/ctrl_module_demo.xml @@ -0,0 +1,30 @@ + + + + + + Demo Control Module + + Rate-controller as module sample + + + + + + + + + + + + +
+ +
+ + + + + + + diff --git a/conf/modules/cv_opticflow.xml b/conf/modules/cv_opticflow.xml new file mode 100644 index 0000000000..af3f680a58 --- /dev/null +++ b/conf/modules/cv_opticflow.xml @@ -0,0 +1,75 @@ + + + + + + Compute Optic Flow from Ardrone2 Bottom Camera + + Computes Pitch- and rollrate corrected optic flow from downward looking + ARDrone2 camera looking at a textured floor. + + - Sonar is required. + - Controller can hold position + + +
+ + + + + + + +
+ + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/modules/extra_dl.xml b/conf/modules/extra_dl.xml index 7351d9f0b5..e03ed7ef73 100644 --- a/conf/modules/extra_dl.xml +++ b/conf/modules/extra_dl.xml @@ -9,6 +9,7 @@
+ diff --git a/conf/modules/nav_bungee_takeoff.xml b/conf/modules/nav_bungee_takeoff.xml index b701b262cc..0d5e2e6332 100644 --- a/conf/modules/nav_bungee_takeoff.xml +++ b/conf/modules/nav_bungee_takeoff.xml @@ -3,18 +3,20 @@ - Takeoff functions for bungee takeoff. - Run initialize function when the plane is on the bungee, the bungee is fully extended and you are ready to - launch the plane. After initialized, the plane will follow a line drawn by the position of the plane on initialization and the - position of the bungee (given in the arguments). Once the plane crosses the throttle line, which is perpendicular to the line the plane is following, - and intersects the position of the bungee (plus or minus a fixed distance (TakeOff_Distance in airframe file) from the bungee just in case the bungee doesn't release directly above the bungee) the prop will come on. The plane will then continue to follow the line until it has reached a specific - height (defined in as Takeoff_Height in airframe file) above the bungee waypoint and speed (defined as Takeoff_Speed in the airframe file). + Takeoff functions for bungee takeoff. + + Run initialize function when the plane is on the bungee, the bungee is fully extended and you are ready to launch the plane. + After initialized, the plane will follow a line drawn by the position of the plane on initialization and the position of the bungee (given in the arguments). + Once the plane crosses the throttle line, which is perpendicular to the line the plane is following, and intersects the position of the bungee (plus or minus a fixed distance (BUNGEE_TAKEOFF_DISTANCE in airframe file) from the bungee just in case the bungee doesn't release exactly above the bungee) the prop will come on. + The plane will then continue to follow the line until it has reached a specific height (defined in as BUNGEE_TAKEOFF_HEIGHT in airframe file) above the bungee waypoint and airspeed (defined as BUNGEE_TAKEOFF_AIRSPEED in the airframe file). The airspeed limit is only used if USE_AIRSPEED flag is defined or set to true (and assuming the airspeed is then available). It is also possible to specify the pitch angle (BUNGEE_TAKEOFF_PITCH) and the throttle (BUNGEE_TAKEOFF_THROTTLE, between 0 and 1). -
- - - - +
+ + + + + +
diff --git a/conf/modules/video_usb_logger.xml b/conf/modules/video_usb_logger.xml new file mode 100644 index 0000000000..77a60e29b2 --- /dev/null +++ b/conf/modules/video_usb_logger.xml @@ -0,0 +1,24 @@ + + + + + + Log video and pose to USB-stick. + Logs attitude and position to a csv and images to jpeg files (only for linux). + + + + video_rtp_stream +
+ +
+ + + + + + + + + + diff --git a/sw/airborne/arch/stm32/usb_ser_hw.c b/sw/airborne/arch/stm32/usb_ser_hw.c index bfbfae9911..0ad2afc1f3 100644 --- a/sw/airborne/arch/stm32/usb_ser_hw.c +++ b/sw/airborne/arch/stm32/usb_ser_hw.c @@ -35,6 +35,7 @@ #include #include #include +#include #include "mcu_periph/usb_serial.h" @@ -61,8 +62,6 @@ bool_t fifo_put(fifo_t *fifo, uint8_t c); bool_t fifo_get(fifo_t *fifo, uint8_t *pc); int fifo_avail(fifo_t *fifo); int fifo_free(fifo_t *fifo); -inline char *get_dev_unique_id(char *serial_no); - usbd_device *my_usbd_dev; @@ -215,31 +214,6 @@ static const char *usb_strings[] = { serial_no, }; -/** - * Serial is 96bit so 12bytes so 12 hexa numbers, or 24 decimal + termination character - */ -inline char *get_dev_unique_id(char *s) -{ -#if defined STM32F4 - volatile uint8_t *unique_id = (volatile uint8_t *)0x1FFF7A10; -#else - volatile uint8_t *unique_id = (volatile uint8_t *)0x1FFFF7E8; -#endif - int i; - - // Fetch serial number from chip's unique ID - for (i = 0; i < 24; i += 2) { - s[i] = ((*unique_id >> 4) & 0xF) + '0'; - s[i + 1] = (*unique_id++ & 0xF) + '0'; - } - for (i = 0; i < 24; i++) - if (s[i] > '9') { - s[i] += 'A' - '9' - 1; - } - // add termination character - s[24] = '\0'; - return s; -} /* * Buffer to be used for control requests. @@ -529,7 +503,7 @@ void VCOM_init(void) rcc_periph_clock_enable(RCC_OTGFS); /* Get serial number */ - get_dev_unique_id(serial_no); + desig_get_unique_id_as_string(serial_no, 25); /* usb driver init*/ my_usbd_dev = usbd_init(&otgfs_usb_driver, &dev, &config, diff --git a/sw/airborne/boards/apogee/baro_board.c b/sw/airborne/boards/apogee/baro_board.c index 84c3df4e42..f889786aec 100644 --- a/sw/airborne/boards/apogee/baro_board.c +++ b/sw/airborne/boards/apogee/baro_board.c @@ -101,9 +101,9 @@ void apogee_baro_event(void) mpl3115_event(&apogee_baro); if (apogee_baro.data_available && startup_cnt == 0) { float pressure = ((float)apogee_baro.pressure / (1 << 2)); - AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure); float temp = apogee_baro.temperature / 16.0f; - AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, &temp); + AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp); apogee_baro.data_available = FALSE; } } diff --git a/sw/airborne/boards/ardrone/baro_board.c b/sw/airborne/boards/ardrone/baro_board.c index 60a745e5f2..939106b936 100644 --- a/sw/airborne/boards/ardrone/baro_board.c +++ b/sw/airborne/boards/ardrone/baro_board.c @@ -95,13 +95,13 @@ void ardrone_baro_event(void) if (baro_calibrated) { // first read temperature because pressure calibration depends on temperature float temp_deg = 0.1 * baro_apply_calibration_temp(navdata.temperature_pressure); - AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, &temp_deg); + AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp_deg); int32_t press_pascal = baro_apply_calibration(navdata.pressure); #if USE_BARO_MEDIAN_FILTER press_pascal = update_median_filter(&baro_median, press_pascal); #endif float pressure = (float)press_pascal; - AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure); } navdata_baro_available = FALSE; } diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c index 5d7a461963..24ff5a3e4a 100644 --- a/sw/airborne/boards/ardrone/navdata.c +++ b/sw/airborne/boards/ardrone/navdata.c @@ -550,7 +550,7 @@ void navdata_update() // Check if there is a new sonar measurement and update the sonar if (navdata.ultrasound >> 15) { float sonar_meas = (float)((navdata.ultrasound & 0x7FFF) - SONAR_OFFSET) * SONAR_SCALE; - AbiSendMsgAGL(AGL_SONAR_ARDRONE2_ID, &sonar_meas); + AbiSendMsgAGL(AGL_SONAR_ARDRONE2_ID, sonar_meas); } #endif diff --git a/sw/airborne/boards/baro_board_ms5611_i2c.c b/sw/airborne/boards/baro_board_ms5611_i2c.c index da5d5a9119..fb5b10eaec 100644 --- a/sw/airborne/boards/baro_board_ms5611_i2c.c +++ b/sw/airborne/boards/baro_board_ms5611_i2c.c @@ -98,9 +98,9 @@ void baro_event(void) if (bb_ms5611.data_available) { float pressure = (float)bb_ms5611.data.pressure; - AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure); float temp = bb_ms5611.data.temperature / 100.0f; - AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, &temp); + AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp); bb_ms5611.data_available = FALSE; #ifdef BARO_LED diff --git a/sw/airborne/boards/baro_board_ms5611_spi.c b/sw/airborne/boards/baro_board_ms5611_spi.c index 7100efa2ba..5b5d78880b 100644 --- a/sw/airborne/boards/baro_board_ms5611_spi.c +++ b/sw/airborne/boards/baro_board_ms5611_spi.c @@ -87,9 +87,9 @@ void baro_event(void) if (bb_ms5611.data_available) { float pressure = (float)bb_ms5611.data.pressure; - AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure); float temp = bb_ms5611.data.temperature / 100.0f; - AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, &temp); + AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp); bb_ms5611.data_available = FALSE; #ifdef BARO_LED diff --git a/sw/airborne/boards/booz/baro_board.c b/sw/airborne/boards/booz/baro_board.c index 8e5c3528f9..ed24d9f412 100644 --- a/sw/airborne/boards/booz/baro_board.c +++ b/sw/airborne/boards/booz/baro_board.c @@ -86,7 +86,7 @@ void baro_periodic(void) RunOnceEvery(10, { baro_board_calibrate();}); } else { float pressure = 101325.0 - BOOZ_BARO_SENS * (BOOZ_ANALOG_BARO_THRESHOLD - baro_board.absolute); - AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure); } } diff --git a/sw/airborne/boards/hbmini/baro_board.c b/sw/airborne/boards/hbmini/baro_board.c index 1bf92a5450..8ab4c42be8 100644 --- a/sw/airborne/boards/hbmini/baro_board.c +++ b/sw/airborne/boards/hbmini/baro_board.c @@ -57,7 +57,7 @@ void bmp_baro_event(void) if (baro_bmp085.data_available) { float pressure = (float)baro_bmp085.pressure; - AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure); baro_bmp085.data_available = FALSE; #ifdef BARO_LED RunOnceEvery(10, LED_TOGGLE(BARO_LED)); diff --git a/sw/airborne/boards/lisa_l/baro_board.c b/sw/airborne/boards/lisa_l/baro_board.c index 54cf3c5dbe..0722d18512 100644 --- a/sw/airborne/boards/lisa_l/baro_board.c +++ b/sw/airborne/boards/lisa_l/baro_board.c @@ -139,7 +139,7 @@ void lisa_l_baro_event(void) if (baro_trans.status == I2CTransSuccess) { int16_t tmp = baro_trans.buf[0] << 8 | baro_trans.buf[1]; float pressure = LISA_L_BARO_SENS * (float)tmp; - AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure); } } else if (baro_board.status == LBS_READING_DIFF && baro_trans.status != I2CTransPending) { @@ -147,7 +147,7 @@ void lisa_l_baro_event(void) if (baro_trans.status == I2CTransSuccess) { int16_t tmp = baro_trans.buf[0] << 8 | baro_trans.buf[1]; float diff = LISA_L_DIFF_SENS * (float)tmp; - AbiSendMsgBARO_DIFF(BARO_BOARD_SENDER_ID, &diff); + AbiSendMsgBARO_DIFF(BARO_BOARD_SENDER_ID, diff); } } } diff --git a/sw/airborne/boards/lisa_m/baro_board.c b/sw/airborne/boards/lisa_m/baro_board.c index 9ffbca3673..43b272e484 100644 --- a/sw/airborne/boards/lisa_m/baro_board.c +++ b/sw/airborne/boards/lisa_m/baro_board.c @@ -75,9 +75,9 @@ void baro_event(void) if (baro_bmp085.data_available) { float pressure = (float)baro_bmp085.pressure; - AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure); float temp = baro_bmp085.temperature / 10.0f; - AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, &temp); + AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp); baro_bmp085.data_available = FALSE; #ifdef BARO_LED RunOnceEvery(10, LED_TOGGLE(BARO_LED)); diff --git a/sw/airborne/boards/navgo/baro_board.c b/sw/airborne/boards/navgo/baro_board.c index 95e04e2310..a9ff7e5290 100644 --- a/sw/airborne/boards/navgo/baro_board.c +++ b/sw/airborne/boards/navgo/baro_board.c @@ -86,7 +86,7 @@ void navgo_baro_event(void) if (startup_cnt == 0) { // Send data when init phase is done float pressure = NAVGO_BARO_SENS * (mcp355x_data + NAVGO_BARO_OFFSET); - AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure); } mcp355x_data_available = FALSE; } diff --git a/sw/airborne/boards/navstik/baro_board.c b/sw/airborne/boards/navstik/baro_board.c index 8327543b43..3dd191e13f 100644 --- a/sw/airborne/boards/navstik/baro_board.c +++ b/sw/airborne/boards/navstik/baro_board.c @@ -60,9 +60,9 @@ void baro_event(void) if (baro_bmp085.data_available) { float pressure = (float)baro_bmp085.pressure; - AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure); float temp = baro_bmp085.temperature / 10.0f; - AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, &temp); + AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp); baro_bmp085.data_available = FALSE; #ifdef BARO_LED RunOnceEvery(10, LED_TOGGLE(BARO_LED)); diff --git a/sw/airborne/boards/umarim/baro_board.c b/sw/airborne/boards/umarim/baro_board.c index 63ffb90361..613f52ed37 100644 --- a/sw/airborne/boards/umarim/baro_board.c +++ b/sw/airborne/boards/umarim/baro_board.c @@ -78,7 +78,7 @@ void umarim_baro_event(void) if (BARO_ABS_ADS.data_available) { if (startup_cnt == 0) { float pressure = UMARIM_BARO_SENS * Ads1114GetValue(BARO_ABS_ADS); - AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure); } BARO_ABS_ADS.data_available = FALSE; } diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index fffdd5e786..b23a9803a0 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -423,6 +423,11 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) case AP_MODE_NAV: guidance_h_mode_changed(GUIDANCE_H_MODE_NAV); break; + case AP_MODE_MODULE: +#ifdef GUIDANCE_H_MODE_MODULE_SETTING + guidance_h_mode_changed(GUIDANCE_H_MODE_MODULE_SETTING); +#endif + break; default: break; } @@ -464,6 +469,11 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) case AP_MODE_NAV: guidance_v_mode_changed(GUIDANCE_V_MODE_NAV); break; + case AP_MODE_MODULE: +#ifdef GUIDANCE_V_MODE_MODULE_SETTING + guidance_v_mode_changed(GUIDANCE_V_MODE_MODULE_SETTING); +#endif + break; default: break; } diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.h b/sw/airborne/firmwares/rotorcraft/autopilot.h index 75a52806e7..8ee77b2aff 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot.h @@ -50,6 +50,7 @@ #define AP_MODE_RC_DIRECT 14 // Safety Pilot Direct Commands for helicopter direct control #define AP_MODE_CARE_FREE_DIRECT 15 #define AP_MODE_FORWARD 16 +#define AP_MODE_MODULE 17 extern uint8_t autopilot_mode; extern uint8_t autopilot_mode_auto2; diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index bfed116024..0737340fe8 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -27,6 +27,7 @@ #include "generated/airframe.h" #include "firmwares/rotorcraft/guidance/guidance_h.h" +#include "firmwares/rotorcraft/guidance/guidance_module.h" #include "firmwares/rotorcraft/stabilization.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h" #include "firmwares/rotorcraft/navigation.h" @@ -252,6 +253,12 @@ void guidance_h_mode_changed(uint8_t new_mode) stabilization_attitude_enter(); break; +#if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE + case GUIDANCE_H_MODE_MODULE: + guidance_h_module_enter(); + break; +#endif + case GUIDANCE_H_MODE_NAV: guidance_h_nav_enter(); #if NO_ATTITUDE_RESET_ON_MODE_CHANGE @@ -304,6 +311,12 @@ void guidance_h_read_rc(bool_t in_flight) #endif break; +#if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE + case GUIDANCE_H_MODE_MODULE: + guidance_h_module_read_rc(); + break; +#endif + case GUIDANCE_H_MODE_NAV: if (radio_control.status == RC_OK) { stabilization_attitude_read_rc_setpoint_eulers(&guidance_h_rc_sp, in_flight, FALSE, FALSE); @@ -387,6 +400,12 @@ void guidance_h_run(bool_t in_flight) stabilization_attitude_run(in_flight); break; +#if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE + case GUIDANCE_H_MODE_MODULE: + guidance_h_module_run(in_flight); + break; +#endif + default: break; } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h index ac9b064017..626ae4a7e8 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h @@ -57,6 +57,7 @@ #define GUIDANCE_H_MODE_RC_DIRECT 5 #define GUIDANCE_H_MODE_CARE_FREE 6 #define GUIDANCE_H_MODE_FORWARD 7 +#define GUIDANCE_H_MODE_MODULE 8 extern uint8_t guidance_h_mode; diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h new file mode 100644 index 0000000000..f0d7cf8aa4 --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2015 + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can 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, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file firmwares/rotorcraft/guidance/guidance_module.h + * Guidance in a module file. + * + * Implement a custom controller in a module. + * Re-use desired modes: + * + * e.g.: #define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_HOVER + * can be used to only define a horizontal control in the module and use normal z_hold + * + * The guidance that the module implement must be activated with following defines: + * + * a) Implement own Horizontal loops when GUIDANCE_H_MODE_MODULE_SETTING is set to GUIDANCE_H_MODE_MODULE + * One must then implement: + * - void guidance_h_module_enter(void); + * - void guidance_h_module_read_rc(void); + * - void guidance_h_module_run(bool_t in_flight); + * + * + * b) Implement own Vertical loops when GUIDANCE_V_MODE_MODULE_SETTING is set to GUIDANCE_V_MODE_MODULE + * - void guidance_v_module_enter(void); + * - void guidance_v_module_run(bool_t in_flight); + * + * If the module implements both V and H mode, take into account that the H is called first and later V + * + */ + +#ifndef GUIDANCE_MODULE_H_ +#define GUIDANCE_MODULE_H_ + +#include "generated/modules.h" + + +#endif /* GUIDANCE_MODULE_H_ */ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index ec8ab3082f..c6d74dae38 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -26,6 +26,7 @@ #include "generated/airframe.h" #include "firmwares/rotorcraft/guidance/guidance_v.h" +#include "firmwares/rotorcraft/guidance/guidance_module.h" #include "subsystems/radio_control.h" #include "firmwares/rotorcraft/stabilization.h" @@ -237,6 +238,12 @@ void guidance_v_mode_changed(uint8_t new_mode) GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0); break; +#if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE + case GUIDANCE_V_MODE_MODULE: + guidance_v_module_enter(); + break; +#endif + default: break; @@ -307,6 +314,12 @@ void guidance_v_run(bool_t in_flight) stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; break; +#if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE + case GUIDANCE_V_MODE_MODULE: + guidance_v_module_run(in_flight); + break; +#endif + case GUIDANCE_V_MODE_NAV: { if (vertical_mode == VERTICAL_MODE_ALT) { guidance_v_z_sp = -nav_flight_altitude; diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h index ee836ec83e..49053be7fd 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h @@ -38,6 +38,7 @@ #define GUIDANCE_V_MODE_CLIMB 3 #define GUIDANCE_V_MODE_HOVER 4 #define GUIDANCE_V_MODE_NAV 5 +#define GUIDANCE_V_MODE_MODULE 6 extern uint8_t guidance_v_mode; diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index 187f37462d..b4520fd23b 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -48,9 +48,6 @@ #include "messages.h" #include "mcu_periph/uart.h" -const uint8_t nb_waypoint = NB_WAYPOINT; -struct EnuCoor_i waypoints[NB_WAYPOINT]; - struct EnuCoor_i navigation_target; struct EnuCoor_i navigation_carrot; @@ -136,30 +133,23 @@ static void send_wp_moved(struct transport_tx *trans, struct link_device *dev) if (i >= nb_waypoint) { i = 0; } pprz_msg_send_WP_MOVED_ENU(trans, dev, AC_ID, &i, - &(waypoints[i].x), - &(waypoints[i].y), - &(waypoints[i].z)); + &(waypoints[i].enu_i.x), + &(waypoints[i].enu_i.y), + &(waypoints[i].enu_i.z)); } #endif void nav_init(void) { - // convert to - const struct EnuCoor_f wp_tmp_float[NB_WAYPOINT] = WAYPOINTS_ENU; - // init int32 waypoints - uint8_t i = 0; - for (i = 0; i < nb_waypoint; i++) { - waypoints[i].x = POS_BFP_OF_REAL(wp_tmp_float[i].x); - waypoints[i].y = POS_BFP_OF_REAL(wp_tmp_float[i].y); - waypoints[i].z = POS_BFP_OF_REAL(wp_tmp_float[i].z); - } + waypoints_init(); + nav_block = 0; nav_stage = 0; nav_altitude = POS_BFP_OF_REAL(SECURITY_HEIGHT); nav_flight_altitude = nav_altitude; flight_altitude = SECURITY_ALT; - VECT3_COPY(navigation_target, waypoints[WP_HOME]); - VECT3_COPY(navigation_carrot, waypoints[WP_HOME]); + VECT3_COPY(navigation_target, waypoints[WP_HOME].enu_i); + VECT3_COPY(navigation_carrot, waypoints[WP_HOME].enu_i); horizontal_mode = HORIZONTAL_MODE_WAYPOINT; vertical_mode = VERTICAL_MODE_ALT; @@ -383,12 +373,15 @@ static inline void nav_set_altitude(void) unit_t nav_reset_reference(void) { ins_reset_local_origin(); + /* update local ENU coordinates of global waypoints */ + nav_localize_global_waypoints(); return 0; } unit_t nav_reset_alt(void) { ins_reset_altitude_ref(); + nav_localize_global_waypoints(); return 0; } @@ -414,28 +407,6 @@ void nav_periodic_task(void) nav_run(); } -void nav_move_waypoint_lla(uint8_t wp_id, struct LlaCoor_i *new_lla_pos) -{ - if (stateIsLocalCoordinateValid()) { - struct EnuCoor_i enu; - enu_of_lla_point_i(&enu, &state.ned_origin_i, new_lla_pos); - // convert ENU pos from cm to BFP with INT32_POS_FRAC - enu.x = POS_BFP_OF_REAL(enu.x) / 100; - enu.y = POS_BFP_OF_REAL(enu.y) / 100; - enu.z = POS_BFP_OF_REAL(enu.z) / 100; - nav_move_waypoint(wp_id, &enu); - } -} - -void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i *new_pos) -{ - if (wp_id < nb_waypoint) { - VECT3_COPY(waypoints[wp_id], (*new_pos)); - DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &(new_pos->x), - &(new_pos->y), &(new_pos->z)); - } -} - void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp) { // MY_ASSERT(wp < nb_waypoint); FIXME @@ -446,16 +417,18 @@ void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int struct Int32Vect3 delta_pos; VECT3_SDIV(delta_pos, speed_sp, NAV_FREQ); /* fixme :make sure the division is really a >> */ INT32_VECT3_RSHIFT(delta_pos, delta_pos, (INT32_SPEED_FRAC - INT32_POS_FRAC)); - waypoints[wp].x += (s_heading * delta_pos.x + c_heading * delta_pos.y) >> INT32_TRIG_FRAC; - waypoints[wp].y += (c_heading * delta_pos.x - s_heading * delta_pos.y) >> INT32_TRIG_FRAC; - waypoints[wp].z += delta_pos.z; + waypoints[wp].enu_i.x += (s_heading * delta_pos.x + c_heading * delta_pos.y) >> INT32_TRIG_FRAC; + waypoints[wp].enu_i.y += (c_heading * delta_pos.x - s_heading * delta_pos.y) >> INT32_TRIG_FRAC; + waypoints[wp].enu_i.z += delta_pos.z; int32_t delta_heading = heading_rate_sp / NAV_FREQ; delta_heading = delta_heading >> (INT32_SPEED_FRAC - INT32_POS_FRAC); nav_heading += delta_heading; INT32_COURSE_NORMALIZE(nav_heading); - RunOnceEvery(10, DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp, &(waypoints[wp].x), &(waypoints[wp].y), - &(waypoints[wp].z))); + RunOnceEvery(10, DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp, + &(waypoints[wp].enu_i.x), + &(waypoints[wp].enu_i.y), + &(waypoints[wp].enu_i.z))); } bool_t nav_detect_ground(void) @@ -474,10 +447,10 @@ bool_t nav_is_in_flight(void) void nav_home(void) { horizontal_mode = HORIZONTAL_MODE_WAYPOINT; - VECT3_COPY(navigation_target, waypoints[WP_HOME]); + VECT3_COPY(navigation_target, waypoints[WP_HOME].enu_i); vertical_mode = VERTICAL_MODE_ALT; - nav_altitude = waypoints[WP_HOME].z; + nav_altitude = waypoints[WP_HOME].enu_i.z; nav_flight_altitude = nav_altitude; dist2_to_wp = dist2_to_home; @@ -499,7 +472,7 @@ float get_dist2_to_point(struct EnuCoor_i *p) /** Returns squared horizontal distance to given waypoint */ float get_dist2_to_waypoint(uint8_t wp_id) { - return get_dist2_to_point(&waypoints[wp_id]); + return get_dist2_to_point(&waypoints[wp_id].enu_i); } /** Computes squared distance to the HOME waypoint potentially sets diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h index 16930a506a..d0a809a769 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.h +++ b/sw/airborne/firmwares/rotorcraft/navigation.h @@ -30,8 +30,8 @@ #include "std.h" #include "math/pprz_geodetic_int.h" -#include "math/pprz_geodetic_float.h" +#include "subsystems/navigation/waypoints.h" #include "subsystems/navigation/common_flight_plan.h" #define NAV_FREQ 16 @@ -39,9 +39,6 @@ extern struct EnuCoor_i navigation_target; extern struct EnuCoor_i navigation_carrot; -extern struct EnuCoor_i waypoints[]; -extern const uint8_t nb_waypoint; - extern void nav_init(void); extern void nav_run(void); @@ -84,8 +81,6 @@ extern void nav_home(void); unit_t nav_reset_reference(void) __attribute__((unused)); unit_t nav_reset_alt(void) __attribute__((unused)); void nav_periodic_task(void); -void nav_move_waypoint_lla(uint8_t wp_id, struct LlaCoor_i *new_lla_pos); -void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i *new_pos); bool_t nav_detect_ground(void); bool_t nav_is_in_flight(void); @@ -107,13 +102,7 @@ extern bool_t nav_set_heading_current(void); #define NavSetGroundReferenceHere() ({ nav_reset_reference(); FALSE; }) #define NavSetAltitudeReferenceHere() ({ nav_reset_alt(); FALSE; }) -#define NavSetWaypointHere(_wp) ({ VECT2_COPY(waypoints[_wp], *stateGetPositionEnu_i()); FALSE; }) -#define NavCopyWaypoint(_wp1, _wp2) ({ VECT2_COPY(waypoints[_wp1], waypoints[_wp2]); FALSE; }) - -#define WaypointX(_wp) POS_FLOAT_OF_BFP(waypoints[_wp].x) -#define WaypointY(_wp) POS_FLOAT_OF_BFP(waypoints[_wp].y) -#define WaypointAlt(_wp) POS_FLOAT_OF_BFP(waypoints[_wp].z) -#define Height(_h) (_h) +#define NavSetWaypointHere(_wp) ({ nav_set_waypoint_here_2d(_wp); FALSE; }) /** Normalize a degree angle between 0 and 359 */ #define NormCourse(x) { \ @@ -124,7 +113,7 @@ extern bool_t nav_set_heading_current(void); /*********** Navigation to waypoint *************************************/ #define NavGotoWaypoint(_wp) { \ horizontal_mode = HORIZONTAL_MODE_WAYPOINT; \ - VECT3_COPY(navigation_target, waypoints[_wp]); \ + VECT3_COPY(navigation_target, waypoints[_wp].enu_i); \ dist2_to_wp = get_dist2_to_waypoint(_wp); \ } @@ -132,7 +121,7 @@ extern bool_t nav_set_heading_current(void); extern void nav_circle(struct EnuCoor_i *wp_center, int32_t radius); #define NavCircleWaypoint(_center, _radius) { \ horizontal_mode = HORIZONTAL_MODE_CIRCLE; \ - nav_circle(&waypoints[_center], POS_BFP_OF_REAL(_radius)); \ + nav_circle(&waypoints[_center].enu_i, POS_BFP_OF_REAL(_radius)); \ } #define NavCircleCount() ((float)abs(nav_circle_radians) / INT32_ANGLE_2_PI) @@ -146,25 +135,25 @@ extern void nav_circle(struct EnuCoor_i *wp_center, int32_t radius); extern void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end); #define NavSegment(_start, _end) { \ horizontal_mode = HORIZONTAL_MODE_ROUTE; \ - nav_route(&waypoints[_start], &waypoints[_end]); \ + nav_route(&waypoints[_start].enu_i, &waypoints[_end].enu_i); \ } /** Nav glide routine */ #define NavGlide(_last_wp, _wp) { \ - int32_t start_alt = waypoints[_last_wp].z; \ - int32_t diff_alt = waypoints[_wp].z - start_alt; \ + int32_t start_alt = waypoints[_last_wp].enu_i.z; \ + int32_t diff_alt = waypoints[_wp].enu_i.z - start_alt; \ int32_t alt = start_alt + ((diff_alt * nav_leg_progress) / nav_leg_length); \ NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(alt),0); \ } /** Proximity tests on approaching a wp */ bool_t nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time); -#define NavApproaching(wp, time) nav_approaching_from(&waypoints[wp], NULL, time) -#define NavApproachingFrom(wp, from, time) nav_approaching_from(&waypoints[wp], &waypoints[from], time) +#define NavApproaching(wp, time) nav_approaching_from(&waypoints[wp].enu_i, NULL, time) +#define NavApproachingFrom(wp, from, time) nav_approaching_from(&waypoints[wp].enu_i, &waypoints[from].enu_i, time) /** Check the time spent in a radius of 'ARRIVED_AT_WAYPOINT' around a wp */ bool_t nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time); -#define NavCheckWaypointTime(wp, time) nav_check_wp_time(&waypoints[wp], time) +#define NavCheckWaypointTime(wp, time) nav_check_wp_time(&waypoints[wp].enu_i, time) /** Set the climb control to auto-throttle with the specified pitch pre-command */ diff --git a/sw/airborne/modules/air_data/air_data.c b/sw/airborne/modules/air_data/air_data.c index 41a4945bdc..5b2d8efb6c 100644 --- a/sw/airborne/modules/air_data/air_data.c +++ b/sw/airborne/modules/air_data/air_data.c @@ -95,9 +95,9 @@ PRINT_CONFIG_MSG("USE_AIRSPEED_AIR_DATA automatically set to TRUE") static uint8_t baro_health_counter; -static void pressure_abs_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) +static void pressure_abs_cb(uint8_t __attribute__((unused)) sender_id, float pressure) { - air_data.pressure = *pressure; + air_data.pressure = pressure; // calculate QNH from pressure and absolute alitude if that is available if (air_data.calc_qnh_once && stateIsGlobalCoordinateValid()) { @@ -116,9 +116,9 @@ static void pressure_abs_cb(uint8_t __attribute__((unused)) sender_id, const flo baro_health_counter = 10; } -static void pressure_diff_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) +static void pressure_diff_cb(uint8_t __attribute__((unused)) sender_id, float pressure) { - air_data.differential = *pressure; + air_data.differential = pressure; if (air_data.calc_airspeed) { air_data.airspeed = tas_from_dynamic_pressure(air_data.differential); #if USE_AIRSPEED_AIR_DATA @@ -127,9 +127,9 @@ static void pressure_diff_cb(uint8_t __attribute__((unused)) sender_id, const fl } } -static void temperature_cb(uint8_t __attribute__((unused)) sender_id, const float *temp) +static void temperature_cb(uint8_t __attribute__((unused)) sender_id, float temp) { - air_data.temperature = *temp; + air_data.temperature = temp; if (air_data.calc_tas_factor && baro_health_counter > 0 && air_data.pressure > 0) { air_data.tas_factor = get_tas_factor(air_data.pressure, air_data.temperature); } diff --git a/sw/airborne/modules/computer_vision/cv/framerate.c b/sw/airborne/modules/computer_vision/cv/framerate.c new file mode 100644 index 0000000000..9ec262d0bc --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv/framerate.c @@ -0,0 +1,76 @@ +/* + * Copyright (C) 2015 The Paparazzi Community + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can 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, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file modules/computer_vision/cv/framerate.c + * + */ + +#include "std.h" +#include "framerate.h" + +// Frame Rate (FPS) +#include + +// local variables +volatile long timestamp; +struct timeval start_time; +struct timeval end_time; + +#define USEC_PER_SEC 1000000L + +static long time_elapsed(struct timeval *t1, struct timeval *t2) +{ + long sec, usec; + sec = t2->tv_sec - t1->tv_sec; + usec = t2->tv_usec - t1->tv_usec; + if (usec < 0) { + --sec; + usec = usec + USEC_PER_SEC; + } + return sec * USEC_PER_SEC + usec; +} + +static void start_timer(void) +{ + gettimeofday(&start_time, NULL); +} + +static long end_timer(void) +{ + gettimeofday(&end_time, NULL); + return time_elapsed(&start_time, &end_time); +} + + +void framerate_init(void) { + // Frame Rate Initialization + timestamp = 0; + start_timer(); +} + +float framerate_run(void) { + // FPS + timestamp = end_timer(); + float framerate_FPS = (float) 1000000 / (float)timestamp; + // printf("dt = %d, FPS = %f\n",timestamp, FPS); + start_timer(); + return framerate_FPS; +} diff --git a/sw/airborne/modules/computer_vision/cv/framerate.h b/sw/airborne/modules/computer_vision/cv/framerate.h new file mode 100644 index 0000000000..9044881ff0 --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv/framerate.h @@ -0,0 +1,28 @@ +/* + * Copyright (C) 2015 The Paparazzi Community + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can 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, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file modules/computer_vision/cv/framerate.h + * + */ + + +void framerate_init(void); +float framerate_run(void); diff --git a/sw/airborne/modules/computer_vision/cv/opticflow/fast9/LICENSE b/sw/airborne/modules/computer_vision/cv/opticflow/fast9/LICENSE new file mode 100644 index 0000000000..63a85126e5 --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv/opticflow/fast9/LICENSE @@ -0,0 +1,30 @@ +Copyright(c) 2006, 2008 Edward Rosten +All rights reserved. + +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 +notice, this list of conditions and the following disclaimer. + +*Redistributions in binary form must reproduce the above copyright +notice, this list of conditions and the following disclaimer in the +documentation and / or other materials provided with the distribution. + +*Neither the name of the University of Cambridge nor the names of +its contributors may be used to endorse or promote products derived +from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, +EXEMPLARY, OR CONSEQUENTIAL DAMAGES(INCLUDING, BUT NOT LIMITED TO, + PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT(INCLUDING + NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.c b/sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.c new file mode 100644 index 0000000000..a8d856f9b3 --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.c @@ -0,0 +1,7320 @@ +/* +Copyright (c) 2006, 2008 Edward Rosten +All rights reserved. + +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 + notice, this list of conditions and the following disclaimer. + + *Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + + *Neither the name of the University of Cambridge nor the names of + its contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, +EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#include +#include "fastRosten.h" + +#define Compare(X, Y) ((X)>=(Y)) + +xyFAST *fast9_detect_nonmax(const byte *im, int xsize, int ysize, int stride, int b, int *ret_num_corners) +{ + xyFAST *corners; + int num_corners; + int *scores; + xyFAST *nonmax; + + corners = fast9_detect(im, xsize, ysize, stride, b, &num_corners); + scores = fast9_score(im, stride, corners, num_corners, b); + nonmax = nonmax_suppression(corners, scores, num_corners, ret_num_corners); + + free(corners); + free(scores); + + return nonmax; +} + +xyFAST *nonmax_suppression(const xyFAST *corners, const int *scores, int num_corners, int *ret_num_nonmax) +{ + int num_nonmax = 0; + int last_row; + int *row_start; + int i, j; + xyFAST *ret_nonmax; + const int sz = (int)num_corners; + + /*Point above points (roughly) to the pixel above the one of interest, if there + is a feature there.*/ + int point_above = 0; + int point_below = 0; + + + if (num_corners < 1) { + *ret_num_nonmax = 0; + return 0; + } + + ret_nonmax = (xyFAST *)malloc(num_corners * sizeof(xyFAST)); + + /* Find where each row begins + (the corners are output in raster scan order). A beginning of -1 signifies + that there are no corners on that row. */ + last_row = corners[num_corners - 1].y; + row_start = (int *)malloc((last_row + 1) * sizeof(int)); + + for (i = 0; i < last_row + 1; i++) { + row_start[i] = -1; + } + + { + int prev_row = -1; + for (i = 0; i < num_corners; i++) + if (corners[i].y != prev_row) { + row_start[corners[i].y] = i; + prev_row = corners[i].y; + } + } + + + + for (i = 0; i < sz; i++) { + int score = scores[i]; + xyFAST pos = corners[i]; + + /*Check left */ + if (i > 0) + if (corners[i - 1].x == pos.x - 1 && corners[i - 1].y == pos.y && Compare(scores[i - 1], score)) { + continue; + } + + /*Check right*/ + if (i < (sz - 1)) + if (corners[i + 1].x == pos.x + 1 && corners[i + 1].y == pos.y && Compare(scores[i + 1], score)) { + continue; + } + + /*Check above (if there is a valid row above)*/ + if (pos.y != 0 && row_start[pos.y - 1] != -1) { + /*Make sure that current point_above is one + row above.*/ + if (corners[point_above].y < pos.y - 1) { + point_above = row_start[pos.y - 1]; + } + + /*Make point_above point to the first of the pixels above the current point, + if it exists.*/ + for (; corners[point_above].y < pos.y && corners[point_above].x < pos.x - 1; point_above++) + {} + + + for (j = point_above; corners[j].y < pos.y && corners[j].x <= pos.x + 1; j++) { + int x = corners[j].x; + if ((x == pos.x - 1 || x == pos.x || x == pos.x + 1) && Compare(scores[j], score)) { + goto cont; + } + } + + } + + /*Check below (if there is anything below)*/ + if (pos.y != last_row && row_start[pos.y + 1] != -1 && point_below < sz) { /*Nothing below*/ + if (corners[point_below].y < pos.y + 1) { + point_below = row_start[pos.y + 1]; + } + + /* Make point below point to one of the pixels belowthe current point, if it + exists.*/ + for (; point_below < sz && corners[point_below].y == pos.y + 1 && corners[point_below].x < pos.x - 1; point_below++) + {} + + for (j = point_below; j < sz && corners[j].y == pos.y + 1 && corners[j].x <= pos.x + 1; j++) { + int x = corners[j].x; + if ((x == pos.x - 1 || x == pos.x || x == pos.x + 1) && Compare(scores[j], score)) { + goto cont; + } + } + } + + ret_nonmax[num_nonmax++] = corners[i]; +cont: + ; + } + + free(row_start); + *ret_num_nonmax = num_nonmax; + return ret_nonmax; +} + +int fast9_corner_score(const byte *p, const int pixel[], int bstart) +{ + int bmin = bstart; + int bmax = 255; + int b = (bmax + bmin) / 2; + + /*Compute the score using binary search*/ + for (;;) { + int cb = *p + b; + int c_b = *p - b; + + + if (p[pixel[0]] > cb) + if (p[pixel[1]] > cb) + if (p[pixel[2]] > cb) + if (p[pixel[3]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) { + goto is_a_corner; + } else if (p[pixel[15]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else if (p[pixel[7]] < c_b) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else if (p[pixel[14]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[15]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[6]] < c_b) + if (p[pixel[15]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else if (p[pixel[13]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[14]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[13]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[14]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[15]] > cb) { + goto is_a_corner; + } else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[12]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[13]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[14]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[6]] < c_b) { + goto is_a_corner; + } else if (p[pixel[15]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) { + goto is_a_corner; + } else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[12]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[6]] < c_b) { + goto is_a_corner; + } else if (p[pixel[15]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[4]] < c_b) + if (p[pixel[13]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) { + goto is_a_corner; + } else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[12]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[13]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[5]] < c_b) { + goto is_a_corner; + } else if (p[pixel[14]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) { + goto is_a_corner; + } else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[5]] < c_b) { + goto is_a_corner; + } else if (p[pixel[14]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[3]] < c_b) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) { + goto is_a_corner; + } else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[4]] < c_b) { + goto is_a_corner; + } else if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) { + goto is_a_corner; + } else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[4]] < c_b) { + goto is_a_corner; + } else if (p[pixel[13]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[2]] < c_b) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) { + goto is_a_corner; + } else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[3]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[9]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[3]] < c_b) { + goto is_a_corner; + } else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) { + goto is_a_corner; + } else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[3]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[9]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[3]] < c_b) { + goto is_a_corner; + } else if (p[pixel[12]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[1]] < c_b) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) { + goto is_a_corner; + } else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[3]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[2]] > cb) + if (p[pixel[3]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[8]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[3]] < c_b) + if (p[pixel[2]] < c_b) { + goto is_a_corner; + } else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) { + goto is_a_corner; + } else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[3]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[2]] > cb) + if (p[pixel[3]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[8]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[3]] < c_b) + if (p[pixel[2]] < c_b) { + goto is_a_corner; + } else if (p[pixel[11]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[0]] < c_b) + if (p[pixel[1]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[3]] > cb) + if (p[pixel[2]] > cb) { + goto is_a_corner; + } else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) { + goto is_a_corner; + } else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[3]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[2]] < c_b) + if (p[pixel[3]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[1]] < c_b) + if (p[pixel[2]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[3]] > cb) { + goto is_a_corner; + } else if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) { + goto is_a_corner; + } else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[3]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[2]] < c_b) + if (p[pixel[3]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[4]] > cb) { + goto is_a_corner; + } else if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) { + goto is_a_corner; + } else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[3]] < c_b) + if (p[pixel[4]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[5]] > cb) { + goto is_a_corner; + } else if (p[pixel[14]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[13]] < c_b) + if (p[pixel[11]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[12]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) { + goto is_a_corner; + } else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[4]] < c_b) + if (p[pixel[5]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[6]] > cb) { + goto is_a_corner; + } else if (p[pixel[15]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[14]] < c_b) + if (p[pixel[12]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[13]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[15]] < c_b) { + goto is_a_corner; + } else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] > cb) + if (p[pixel[15]] < c_b) + if (p[pixel[13]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[14]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[6]] < c_b) + if (p[pixel[7]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[15]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) { + goto is_a_corner; + } else if (p[pixel[15]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[13]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[12]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[6]] > cb) { + goto is_a_corner; + } else if (p[pixel[15]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) { + goto is_a_corner; + } else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[11]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[5]] > cb) { + goto is_a_corner; + } else if (p[pixel[14]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) { + goto is_a_corner; + } else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[10]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[4]] > cb) { + goto is_a_corner; + } else if (p[pixel[13]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) { + goto is_a_corner; + } else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[9]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[3]] > cb) { + goto is_a_corner; + } else if (p[pixel[12]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) { + goto is_a_corner; + } else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[3]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[8]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[3]] > cb) + if (p[pixel[2]] > cb) { + goto is_a_corner; + } else if (p[pixel[11]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) { + goto is_a_corner; + } else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[3]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[2]] < c_b) + if (p[pixel[3]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[3]] > cb) + if (p[pixel[2]] > cb) + if (p[pixel[1]] > cb) { + goto is_a_corner; + } else if (p[pixel[10]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[3]] < c_b) + if (p[pixel[2]] < c_b) + if (p[pixel[1]] < c_b) { + goto is_a_corner; + } else if (p[pixel[10]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) { + goto is_a_corner; + } else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + else { + goto is_not_a_corner; + } + +is_a_corner: + bmin = b; + goto end_if; + +is_not_a_corner: + bmax = b; + goto end_if; + +end_if: + + if (bmin == bmax - 1 || bmin == bmax) { + return bmin; + } + b = (bmin + bmax) / 2; + } +} + +static void make_offsets(int pixel[], int row_stride) +{ + pixel[0] = 0 + row_stride * 3; + pixel[1] = 1 + row_stride * 3; + pixel[2] = 2 + row_stride * 2; + pixel[3] = 3 + row_stride * 1; + pixel[4] = 3 + row_stride * 0; + pixel[5] = 3 + row_stride * -1; + pixel[6] = 2 + row_stride * -2; + pixel[7] = 1 + row_stride * -3; + pixel[8] = 0 + row_stride * -3; + pixel[9] = -1 + row_stride * -3; + pixel[10] = -2 + row_stride * -2; + pixel[11] = -3 + row_stride * -1; + pixel[12] = -3 + row_stride * 0; + pixel[13] = -3 + row_stride * 1; + pixel[14] = -2 + row_stride * 2; + pixel[15] = -1 + row_stride * 3; +} + + + +int *fast9_score(const byte *i, int stride, xyFAST *corners, int num_corners, int b) +{ + int *scores = (int *)malloc(sizeof(int) * num_corners); + int n; + + int pixel[16]; + make_offsets(pixel, stride); + + for (n = 0; n < num_corners; n++) { + scores[n] = fast9_corner_score(i + corners[n].y * stride + corners[n].x, pixel, b); + } + + return scores; +} + + +xyFAST *fast9_detect(const byte *im, int xsize, int ysize, int stride, int b, int *ret_num_corners) +{ + int num_corners = 0; + xyFAST *ret_corners; + int rsize = 512; + int pixel[16]; + int x, y; + + ret_corners = (xyFAST *)malloc(sizeof(xyFAST) * rsize); + make_offsets(pixel, stride); + + for (y = 3; y < ysize - 3; y++) + for (x = 3; x < xsize - 3; x++) { + const byte *p = im + y * stride + x; + + int cb = *p + b; + int c_b = *p - b; + if (p[pixel[0]] > cb) + if (p[pixel[1]] > cb) + if (p[pixel[2]] > cb) + if (p[pixel[3]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + {} + else if (p[pixel[15]] > cb) + {} + else { + continue; + } + else if (p[pixel[7]] < c_b) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else { + continue; + } + else if (p[pixel[14]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[15]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else { + continue; + } + else { + continue; + } + else if (p[pixel[6]] < c_b) + if (p[pixel[15]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + {} + else { + continue; + } + else if (p[pixel[13]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[14]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else { + continue; + } + else { + continue; + } + else if (p[pixel[13]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[14]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[15]] > cb) + {} + else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[12]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[13]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[14]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[6]] < c_b) + {} + else if (p[pixel[15]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[12]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[6]] < c_b) + {} + else if (p[pixel[15]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[4]] < c_b) + if (p[pixel[13]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[12]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[13]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[5]] < c_b) + {} + else if (p[pixel[14]] < c_b) + {} + else { + continue; + } + else if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[5]] < c_b) + {} + else if (p[pixel[14]] < c_b) + {} + else { + continue; + } + else if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[3]] < c_b) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[4]] < c_b) + {} + else if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[4]] < c_b) + {} + else if (p[pixel[13]] < c_b) + {} + else { + continue; + } + else if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[2]] < c_b) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[3]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[9]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[3]] < c_b) + {} + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[3]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[9]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[3]] < c_b) + {} + else if (p[pixel[12]] < c_b) + {} + else { + continue; + } + else if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[1]] < c_b) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + {} + else { + continue; + } + else { + continue; + } + else if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[3]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[2]] > cb) + if (p[pixel[3]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[8]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[3]] < c_b) + if (p[pixel[2]] < c_b) + {} + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + {} + else { + continue; + } + else { + continue; + } + else if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[3]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[2]] > cb) + if (p[pixel[3]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[8]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[3]] < c_b) + if (p[pixel[2]] < c_b) + {} + else if (p[pixel[11]] < c_b) + {} + else { + continue; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[0]] < c_b) + if (p[pixel[1]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[3]] > cb) + if (p[pixel[2]] > cb) + {} + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + {} + else { + continue; + } + else { + continue; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[3]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[2]] < c_b) + if (p[pixel[3]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[1]] < c_b) + if (p[pixel[2]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[3]] > cb) + {} + else if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + {} + else { + continue; + } + else { + continue; + } + else if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[3]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[2]] < c_b) + if (p[pixel[3]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[4]] > cb) + {} + else if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + {} + else { + continue; + } + else { + continue; + } + else if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[3]] < c_b) + if (p[pixel[4]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[5]] > cb) + {} + else if (p[pixel[14]] > cb) + {} + else { + continue; + } + else if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[13]] < c_b) + if (p[pixel[11]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[12]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[5]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[4]] < c_b) + if (p[pixel[5]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[6]] > cb) + {} + else if (p[pixel[15]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[14]] < c_b) + if (p[pixel[12]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[13]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[15]] < c_b) + {} + else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[6]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] > cb) + if (p[pixel[15]] < c_b) + if (p[pixel[13]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[14]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[6]] < c_b) + if (p[pixel[7]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[15]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + {} + else if (p[pixel[15]] < c_b) + {} + else { + continue; + } + else if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else if (p[pixel[13]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[12]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[6]] > cb) + {} + else if (p[pixel[15]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[11]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[5]] > cb) + {} + else if (p[pixel[14]] > cb) + {} + else { + continue; + } + else if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[10]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[4]] > cb) + {} + else if (p[pixel[13]] > cb) + {} + else { + continue; + } + else if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + {} + else { + continue; + } + else { + continue; + } + else if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[9]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[3]] > cb) + {} + else if (p[pixel[12]] > cb) + {} + else { + continue; + } + else if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + {} + else { + continue; + } + else { + continue; + } + else if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[3]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[8]] > cb) + if (p[pixel[7]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[10]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[3]] > cb) + if (p[pixel[2]] > cb) + {} + else if (p[pixel[11]] > cb) + {} + else { + continue; + } + else if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + {} + else { + continue; + } + else { + continue; + } + else if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[3]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[2]] < c_b) + if (p[pixel[3]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[7]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[7]] > cb) + if (p[pixel[8]] > cb) + if (p[pixel[9]] > cb) + if (p[pixel[6]] > cb) + if (p[pixel[5]] > cb) + if (p[pixel[4]] > cb) + if (p[pixel[3]] > cb) + if (p[pixel[2]] > cb) + if (p[pixel[1]] > cb) + {} + else if (p[pixel[10]] > cb) + {} + else { + continue; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + {} + else { + continue; + } + else { + continue; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[10]] > cb) + if (p[pixel[11]] > cb) + if (p[pixel[12]] > cb) + if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[7]] < c_b) + if (p[pixel[8]] < c_b) + if (p[pixel[9]] < c_b) + if (p[pixel[6]] < c_b) + if (p[pixel[5]] < c_b) + if (p[pixel[4]] < c_b) + if (p[pixel[3]] < c_b) + if (p[pixel[2]] < c_b) + if (p[pixel[1]] < c_b) + {} + else if (p[pixel[10]] < c_b) + {} + else { + continue; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else if (p[pixel[10]] < c_b) + if (p[pixel[11]] < c_b) + if (p[pixel[12]] < c_b) + if (p[pixel[13]] < c_b) + if (p[pixel[14]] < c_b) + if (p[pixel[15]] < c_b) + {} + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + else { + continue; + } + if (num_corners == rsize) { + rsize *= 2; + ret_corners = (xyFAST *)realloc(ret_corners, sizeof(xyFAST) * rsize); + } + ret_corners[num_corners].x = x; + ret_corners[num_corners].y = y; + num_corners++; + + } + + *ret_num_corners = num_corners; + return ret_corners; + +} + diff --git a/sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.h b/sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.h new file mode 100644 index 0000000000..683bee37c7 --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.h @@ -0,0 +1,51 @@ +/* +Copyright (c) 2006, 2008 Edward Rosten +All rights reserved. + +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 + notice, this list of conditions and the following disclaimer. + + *Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + + *Neither the name of the University of Cambridge nor the names of + its contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR +CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, +EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, +PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef FAST_H +#define FAST_H + +typedef struct { int x, y; } xyFAST; +typedef unsigned char byte; + +int fast9_corner_score(const byte *p, const int pixel[], int bstart); + +xyFAST *fast9_detect(const byte *im, int xsize, int ysize, int stride, int b, int *ret_num_corners); + +int *fast9_score(const byte *i, int stride, xyFAST *corners, int num_corners, int b); + +xyFAST *fast9_detect_nonmax(const byte *im, int xsize, int ysize, int stride, int b, int *ret_num_corners); + +xyFAST *nonmax_suppression(const xyFAST *corners, const int *scores, int num_corners, int *ret_num_nonmax); + + +#endif diff --git a/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_int.c b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_int.c new file mode 100644 index 0000000000..b1e385972e --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_int.c @@ -0,0 +1,523 @@ +/* + * Copyright (C) 2014 + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can 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, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file modules/computer_vision/cv/opticflow/optic_flow_int.c + * @brief efficient fixed-point optical-flow + * + * - Initial fixed-point C implementation by G. de Croon + * - Algorithm: Lucas-Kanade by Yves Bouguet + * - Publication: http://robots.stanford.edu/cs223b04/algo_tracking.pdf + */ + +#include +#include +#include +#include +#include "optic_flow_int.h" +#include "modules/computer_vision/opticflow_module.h" + +#define int_index(x,y) (y * IMG_WIDTH + x) +#define uint_index(xx, yy) (((yy * IMG_WIDTH + xx) * 2) & 0xFFFFFFFC) +#define NO_MEMORY -1 +#define OK 0 +#define N_VISUAL_INPUTS 51 +#define N_ACTIONS 3 +#define MAX_COUNT_PT 50 + +unsigned int IMG_WIDTH, IMG_HEIGHT; + +void multiplyImages(int *ImA, int *ImB, int *ImC, int width, int height) +{ + int x, y; + unsigned int ix; + + // printf("W = %d, H = %d\n\r", IMG_WIDTH, IMG_HEIGHT); + + for (x = 0; x < width; x++) { + for (y = 0; y < height; y++) { + ix = (y * width + x); + ImC[ix] = ImA[ix] * ImB[ix]; + // If we want to keep the values in [0, 255]: + // ImC[ix] /= 255; + } + } +} + +void getImageDifference(int *ImA, int *ImB, int *ImC, int width, int height) +{ + int x, y; + unsigned int ix; + + // printf("W = %d, H = %d\n\r", IMG_WIDTH, IMG_HEIGHT); + + for (x = 0; x < width; x++) { + for (y = 0; y < height; y++) { + ix = (y * width + x); + ImC[ix] = ImA[ix] - ImB[ix]; + } + } + +} + +void getSubPixel_gray(int *Patch, unsigned char *frame_buf, int center_x, int center_y, int half_window_size, + int subpixel_factor) +{ + int x, y, x_0, y_0, x_0_or, y_0_or, i, j, window_size, alpha_x, alpha_y, max_x, max_y; + unsigned int ix1, ix2, Y; + window_size = half_window_size * 2 + 1; + max_x = (IMG_WIDTH - 1) * subpixel_factor; + max_y = (IMG_HEIGHT - 1) * subpixel_factor; + + for (i = 0; i < window_size; i++) { + for (j = 0; j < window_size; j++) { + // index for this position in the patch: + ix1 = (j * window_size + i); + + // determine subpixel coordinates of the current pixel: + x = center_x + (i - half_window_size) * subpixel_factor; + if (x < 0) { x = 0; } + if (x > max_x) { x = max_x; } + y = center_y + (j - half_window_size) * subpixel_factor; + if (y < 0) { y = 0; } + if (y > max_y) { y = max_y; } + // pixel to the top left: + x_0_or = (x / subpixel_factor); + x_0 = x_0_or * subpixel_factor; + y_0_or = (y / subpixel_factor); + y_0 = y_0_or * subpixel_factor; + + + if (x == x_0 && y == y_0) { + ix2 = y_0_or * IMG_WIDTH + x_0_or; + Y = (unsigned int)frame_buf[ix2 + 1]; + Patch[ix1] = (int) Y; + } else { + // blending according to how far the subpixel coordinates are from the pixel coordinates + alpha_x = (x - x_0); + alpha_y = (y - y_0); + + // the patch pixel is a blend from the four surrounding pixels: + ix2 = y_0_or * IMG_WIDTH + x_0_or; + Y = (unsigned int)frame_buf[ix2 + 1]; + Patch[ix1] = (subpixel_factor - alpha_x) * (subpixel_factor - alpha_y) * ((int) Y); + + ix2 = y_0_or * IMG_WIDTH + (x_0_or + 1); + Y = (unsigned int)frame_buf[ix2 + 1]; + Patch[ix1] += alpha_x * (subpixel_factor - alpha_y) * ((int) Y); + + ix2 = (y_0_or + 1) * IMG_WIDTH + x_0_or; + Y = (unsigned int)frame_buf[ix2 + 1]; + Patch[ix1] += (subpixel_factor - alpha_x) * alpha_y * ((int) Y); + + ix2 = (y_0_or + 1) * IMG_WIDTH + (x_0_or + 1); + Y = (unsigned int)frame_buf[ix2 + 1]; + Patch[ix1] += alpha_x * alpha_y * ((int) Y); + + // normalize patch value + Patch[ix1] /= (subpixel_factor * subpixel_factor); + } + } + } + + return; +} + +void getGradientPatch(int *Patch, int *DX, int *DY, int half_window_size) +{ + unsigned int ix1, ix2; + int x, y, padded_patch_size, patch_size, Y1, Y2; + // int printed; printed = 0; + + padded_patch_size = 2 * (half_window_size + 1) + 1; + patch_size = 2 * half_window_size + 1; + // currently we use [0 0 0; -1 0 1; 0 0 0] as mask for dx + for (x = 1; x < padded_patch_size - 1; x++) { + for (y = 1; y < padded_patch_size - 1; y++) { + // index in DX, DY: + ix2 = (unsigned int)((y - 1) * patch_size + (x - 1)); + + ix1 = (unsigned int)(y * padded_patch_size + x - 1); + Y1 = Patch[ix1]; + ix1 = (unsigned int)(y * padded_patch_size + x + 1); + Y2 = Patch[ix1]; + DX[ix2] = (Y2 - Y1) / 2; + + ix1 = (unsigned int)((y - 1) * padded_patch_size + x); + Y1 = Patch[ix1]; + ix1 = (unsigned int)((y + 1) * padded_patch_size + x); + Y2 = Patch[ix1]; + DY[ix2] = (Y2 - Y1) / 2; + + + } + } + + return; +} + +int getSumPatch(int *Patch, int size) +{ + int x, y, sum; // , threshold + unsigned int ix; + + // in order to keep the sum within range: + //threshold = 50000; // typical values are far below this threshold + sum = 0; + for (x = 0; x < size; x++) { + for (y = 0; y < size; y++) { + ix = (y * size) + x; + sum += Patch[ix]; // do not check thresholds + } + } + + return sum; +} + +int calculateG(int *G, int *DX, int *DY, int half_window_size) +{ + int patch_size; + int *DXX; int *DXY; int *DYY; + + patch_size = 2 * half_window_size + 1; + + // allocate memory: + DXX = (int *) malloc(patch_size * patch_size * sizeof(int)); + DXY = (int *) malloc(patch_size * patch_size * sizeof(int)); + DYY = (int *) malloc(patch_size * patch_size * sizeof(int)); + + if (DXX == 0 || DXY == 0 || DYY == 0) { + return NO_MEMORY; + } + + // then determine the second order gradients + multiplyImages(DX, DX, DXX, patch_size, patch_size); + multiplyImages(DX, DY, DXY, patch_size, patch_size); + multiplyImages(DY, DY, DYY, patch_size, patch_size); + + // calculate G: + G[0] = getSumPatch(DXX, patch_size); + G[1] = getSumPatch(DXY, patch_size); + G[2] = G[1]; + G[3] = getSumPatch(DYY, patch_size); + + // free memory: + free((char *) DXX); free((char *) DXY); free((char *) DYY); + + // no errors: + return OK; +} + + + +int calculateError(int *ImC, int width, int height) +{ + int x, y, error; + unsigned int ix; + + error = 0; + + for (x = 0; x < width; x++) { + for (y = 0; y < height; y++) { + ix = (y * width + x); + error += ImC[ix] * ImC[ix]; + } + } + + return error; +} + +int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int *p_x, int *p_y, int n_found_points, + int imW, int imH, int *new_x, int *new_y, int *status, int half_window_size, int max_iterations) +{ + // A straightforward one-level implementation of Lucas-Kanade. + // For all points: + // (1) determine the subpixel neighborhood in the old image + // (2) get the x- and y- gradients + // (3) determine the 'G'-matrix [sum(Axx) sum(Axy); sum(Axy) sum(Ayy)], where sum is over the window + // (4) iterate over taking steps in the image to minimize the error: + // [a] get the subpixel neighborhood in the new image + // [b] determine the image difference between the two neighborhoods + // [c] calculate the 'b'-vector + // [d] calculate the additional flow step and possibly terminate the iteration + int p, subpixel_factor, x, y, it, step_threshold, step_x, step_y, v_x, v_y, Det; + int b_x, b_y, patch_size, padded_patch_size, error; + unsigned int ix1, ix2; + int *I_padded_neighborhood; int *I_neighborhood; int *J_neighborhood; + int *DX; int *DY; int *ImDiff; int *IDDX; int *IDDY; + int G[4]; + int error_threshold; + + // set the image width and height + IMG_WIDTH = imW; + IMG_HEIGHT = imH; + // spatial resolution of flow is 1 / subpixel_factor + subpixel_factor = 10; + // determine patch sizes and initialize neighborhoods + patch_size = (2 * half_window_size + 1); + error_threshold = (25 * 25) * (patch_size * patch_size); + + padded_patch_size = (2 * half_window_size + 3); + I_padded_neighborhood = (int *) malloc(padded_patch_size * padded_patch_size * sizeof(int)); + I_neighborhood = (int *) malloc(patch_size * patch_size * sizeof(int)); + J_neighborhood = (int *) malloc(patch_size * patch_size * sizeof(int)); + if (I_padded_neighborhood == 0 || I_neighborhood == 0 || J_neighborhood == 0) { + return NO_MEMORY; + } + + DX = (int *) malloc(patch_size * patch_size * sizeof(int)); + DY = (int *) malloc(patch_size * patch_size * sizeof(int)); + IDDX = (int *) malloc(patch_size * patch_size * sizeof(int)); + IDDY = (int *) malloc(patch_size * patch_size * sizeof(int)); + ImDiff = (int *) malloc(patch_size * patch_size * sizeof(int)); + if (DX == 0 || DY == 0 || ImDiff == 0 || IDDX == 0 || IDDY == 0) { + return NO_MEMORY; + } + + for (p = 0; p < n_found_points; p++) { + // status: point is not yet lost: + status[p] = 1; + + // We want to be able to take steps in the image of 1 / subpixel_factor: + p_x[p] *= subpixel_factor; + p_y[p] *= subpixel_factor; + + // if the pixel is outside the ROI in the image, do not track it: + if (!(p_x[p] > ((half_window_size + 1) * subpixel_factor) && p_x[p] < (IMG_WIDTH - half_window_size) * subpixel_factor + && p_y[p] > ((half_window_size + 1) * subpixel_factor) && p_y[p] < (IMG_HEIGHT - half_window_size)*subpixel_factor)) { + status[p] = 0; + } + + // (1) determine the subpixel neighborhood in the old image + // we determine a padded neighborhood with the aim of subsequent gradient processing: + getSubPixel_gray(I_padded_neighborhood, old_image_buf, p_x[p], p_y[p], half_window_size + 1, subpixel_factor); + + // Also get the original-sized neighborhood + for (x = 1; x < padded_patch_size - 1; x++) { + for (y = 1; y < padded_patch_size - 1; y++) { + ix1 = (y * padded_patch_size + x); + ix2 = ((y - 1) * patch_size + (x - 1)); + I_neighborhood[ix2] = I_padded_neighborhood[ix1]; + } + } + + // (2) get the x- and y- gradients + getGradientPatch(I_padded_neighborhood, DX, DY, half_window_size); + + // (3) determine the 'G'-matrix [sum(Axx) sum(Axy); sum(Axy) sum(Ayy)], where sum is over the window + error = calculateG(G, DX, DY, half_window_size); + if (error == NO_MEMORY) { return NO_MEMORY; } + + for (it = 0; it < 4; it++) { + G[it] /= 255; // to keep values in range + } + // calculate G's determinant: + Det = G[0] * G[3] - G[1] * G[2]; + Det = Det / subpixel_factor; // so that the steps will be expressed in subpixel units + if (Det < 1) { + status[p] = 0; + } + + // (4) iterate over taking steps in the image to minimize the error: + it = 0; + step_threshold = 2; // 0.2 as smallest step (L1) + v_x = 0; + v_y = 0; + step_x = step_threshold + 1; + step_y = step_threshold + 1; + + while (status[p] == 1 && it < max_iterations && (abs(step_x) >= step_threshold || abs(step_y) >= step_threshold)) { + // if the pixel goes outside the ROI in the image, stop tracking: + if (!(p_x[p] + v_x > ((half_window_size + 1) * subpixel_factor) + && p_x[p] + v_x < ((int)IMG_WIDTH - half_window_size) * subpixel_factor + && p_y[p] + v_y > ((half_window_size + 1) * subpixel_factor) + && p_y[p] + v_y < ((int)IMG_HEIGHT - half_window_size)*subpixel_factor)) { + status[p] = 0; + break; + } + + // [a] get the subpixel neighborhood in the new image + // clear J: + for (x = 0; x < patch_size; x++) { + for (y = 0; y < patch_size; y++) { + ix2 = (y * patch_size + x); + J_neighborhood[ix2] = 0; + } + } + + + getSubPixel_gray(J_neighborhood, new_image_buf, p_x[p] + v_x, p_y[p] + v_y, half_window_size, subpixel_factor); + // [b] determine the image difference between the two neighborhoods + getImageDifference(I_neighborhood, J_neighborhood, ImDiff, patch_size, patch_size); + error = calculateError(ImDiff, patch_size, patch_size) / 255; + + if (error > error_threshold && it > max_iterations / 2) { + status[p] = 0; + break; + } + multiplyImages(ImDiff, DX, IDDX, patch_size, patch_size); + b_x = getSumPatch(IDDX, patch_size) / 255; + b_y = getSumPatch(IDDY, patch_size) / 255; + //printf("b_x = %d; b_y = %d;\n\r", b_x, b_y); + // [d] calculate the additional flow step and possibly terminate the iteration + step_x = (G[3] * b_x - G[1] * b_y) / Det; + step_y = (G[0] * b_y - G[2] * b_x) / Det; + v_x += step_x; + v_y += step_y; // - (?) since the origin in the image is in the top left of the image, with y positive pointing down + // next iteration + it++; + } // iteration to find the right window in the new image + + new_x[p] = (p_x[p] + v_x) / subpixel_factor; + new_y[p] = (p_y[p] + v_y) / subpixel_factor; + p_x[p] /= subpixel_factor; + p_y[p] /= subpixel_factor; + } + + + + // free all allocated variables: + free((int *) I_padded_neighborhood); + free((int *) I_neighborhood); + free((int *) J_neighborhood); + free((int *) DX); + free((int *) DY); + free((int *) ImDiff); + free((int *) IDDX); + free((int *) IDDY); + // no errors: + return OK; +} + +void quick_sort(float *a, int n) +{ + if (n < 2) { + return; + } + float p = a[n / 2]; + float *l = a; + float *r = a + n - 1; + while (l <= r) { + if (*l < p) { + l++; + continue; + } + if (*r > p) { + r--; + continue; // we need to check the condition (l <= r) every time we change the value of l or r + } + float t = *l; + *l++ = *r; + *r-- = t; + } + quick_sort(a, r - a + 1); + quick_sort(l, a + n - l); +} + +void quick_sort_int(int *a, int n) +{ + if (n < 2) { + return; + } + int p = a[n / 2]; + int *l = a; + int *r = a + n - 1; + while (l <= r) { + if (*l < p) { + l++; + continue; + } + if (*r > p) { + r--; + continue; + } + int t = *l; + *l++ = *r; + *r-- = t; + } + quick_sort_int(a, r - a + 1); + quick_sort_int(l, a + n - l); +} + +void CvtYUYV2Gray(unsigned char *grayframe, unsigned char *frame, int imW, int imH) +{ + int x, y; + unsigned char *Y, *gray; + for (y = 0; y < imH; y++) { + Y = frame + (imW * 2 * y) + 1; + gray = grayframe + (imW * y); + for (x = 0; x < imW; x += 2) { + gray[x] = *Y; + Y += 2; + gray[x + 1] = *Y; + Y += 2; + } + } +} + +unsigned int OF_buf_point = 0; +unsigned int OF_buf_point2 = 0; +float x_avg, y_avg, x_buf[24], y_buf[24], x_buf2[24], y_buf2[24]; + +void OFfilter(float *OFx, float *OFy, float dx, float dy, int count, int OF_FilterType) +{ + + if (OF_FilterType == 1) { //1. moving average 2. moving median + + x_avg = 0.0; + y_avg = 0.0; + + if (count) { + x_buf[OF_buf_point] = dx; + y_buf[OF_buf_point] = dy; + } else { + x_buf[OF_buf_point] = 0.0; + y_buf[OF_buf_point] = 0.0; + } + OF_buf_point = (OF_buf_point + 1) % 20; + + for (int i = 0; i < 20; i++) { + x_avg += x_buf[i] * 0.05; + y_avg += y_buf[i] * 0.05; + } + + *OFx = x_avg; + *OFy = y_avg; + + } else if (OF_FilterType == 2) { + if (count) { + x_buf2[OF_buf_point2] = dx; + y_buf2[OF_buf_point2] = dy; + } else { + x_buf2[OF_buf_point2] = 0.0; + y_buf2[OF_buf_point2] = 0.0; + } + OF_buf_point2 = (OF_buf_point2 + 1) % 11; // 11 + + quick_sort(x_buf2, 11); // 11 + quick_sort(y_buf2, 11); // 11 + + *OFx = x_buf2[6]; // 6 + *OFy = y_buf2[6]; // 6 + } else { + printf("no filter type selected!\n"); + } +} + diff --git a/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_int.h b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_int.h new file mode 100644 index 0000000000..786916ac62 --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_int.h @@ -0,0 +1,45 @@ +/* + * Copyright (C) 2014 + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can 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, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file modules/computer_vision/cv/opticflow/optic_flow_int.h + * @brief efficient fixed-point optical-flow + * + */ + +#ifndef OPTIC_FLOW_INT_H +#define OPTIC_FLOW_INT_H + +void multiplyImages(int *ImA, int *ImB, int *ImC, int width, int height); +void getImageDifference(int *ImA, int *ImB, int *ImC, int width, int height); +void getSubPixel_gray(int *Patch, unsigned char *frame_buf, int center_x, int center_y, int half_window_size, + int subpixel_factor); +void getGradientPatch(int *Patch, int *DX, int *DY, int half_window_size); +int getSumPatch(int *Patch, int size); +int calculateG(int *G, int *DX, int *DY, int half_window_size); +int calculateError(int *ImC, int width, int height); +int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int *p_x, int *p_y, int n_found_points, + int imW, int imH, int *new_x, int *new_y, int *status, int half_window_size, int max_iterations); +void quick_sort(float *a, int n); +void quick_sort_int(int *a, int n); +void CvtYUYV2Gray(unsigned char *grayframe, unsigned char *frame, int imW, int imH); +void OFfilter(float *OFx, float *OFy, float dx, float dy, int count, int OF_FilterType); + +#endif /* OPTIC_FLOW_INT_H */ diff --git a/sw/airborne/modules/computer_vision/cv/resize.h b/sw/airborne/modules/computer_vision/cv/resize.h index e2bbf7096f..b0524a5270 100644 --- a/sw/airborne/modules/computer_vision/cv/resize.h +++ b/sw/airborne/modules/computer_vision/cv/resize.h @@ -23,26 +23,39 @@ #include #include "image.h" +/** Simplified high-speed low CPU downsample function without averaging + * + * downsample factor must be 1, 2, 4, 8 ... 2^X + * image of typ UYVY expected. Only one color UV per 2 pixels + * + * we keep the UV color of the first pixel pair + * and sample the intensity evenly 1-3-5-7-... or 1-5-9-... + * + * input: u1y1 v1y2 u3y3 v3y4 u5y5 v5y6 u7y7 v7y8 ... + * downsample=1 u1y1 v1y2 u3y3 v3y4 u5y5 v5y6 u7y7 v7y8 ... + * downsample=2 u1y1v1 (skip2) y3 (skip2) u5y5v5 (skip2 y7 (skip2) ... + * downsample=4 u1y1v1 (skip6) y5 (skip6) ... + */ + inline void resize_uyuv(struct img_struct *input, struct img_struct *output, int downsample); inline void resize_uyuv(struct img_struct *input, struct img_struct *output, int downsample) { uint8_t *source = input->buf; uint8_t *dest = output->buf; - int pixelskip = downsample - 1; + int pixelskip = (downsample - 1) * 2; for (int y = 0; y < output->h; y++) { for (int x = 0; x < output->w; x += 2) { // YUYV *dest++ = *source++; // U *dest++ = *source++; // Y - // now skip 3 pixels - if (pixelskip) { source += (pixelskip + 1) * 2; } - *dest++ = *source++; // U *dest++ = *source++; // V - if (pixelskip) { source += (pixelskip - 1) * 2; } + source += pixelskip; + *dest++ = *source++; // Y + source += pixelskip; } - // skip 3 rows - if (pixelskip) { source += pixelskip * input->w * 2; } + // read 1 in every 'downsample' rows, so skip (downsample-1) rows after reading the first + source += (downsample-1) * input->w * 2; } } diff --git a/sw/airborne/modules/computer_vision/cv/trig.c b/sw/airborne/modules/computer_vision/cv/trig.c new file mode 100644 index 0000000000..88a1103f32 --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv/trig.c @@ -0,0 +1,159 @@ +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * Trigonometry + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ + +#include "trig.h" + +static int cosine[] = { + 10000, 9998, 9994, 9986, 9976, 9962, 9945, 9925, 9903, 9877, + 9848, 9816, 9781, 9744, 9703, 9659, 9613, 9563, 9511, 9455, + 9397, 9336, 9272, 9205, 9135, 9063, 8988, 8910, 8829, 8746, + 8660, 8572, 8480, 8387, 8290, 8192, 8090, 7986, 7880, 7771, + 7660, 7547, 7431, 7314, 7193, 7071, 6947, 6820, 6691, 6561, + 6428, 6293, 6157, 6018, 5878, 5736, 5592, 5446, 5299, 5150, + 5000, 4848, 4695, 4540, 4384, 4226, 4067, 3907, 3746, 3584, + 3420, 3256, 3090, 2924, 2756, 2588, 2419, 2250, 2079, 1908, + 1736, 1564, 1392, 1219, 1045, 872, 698, 523, 349, 175, + 0 +}; + +int sin_zelf(int ix) +{ + while (ix < 0) { + ix = ix + 360; + } + while (ix >= 360) { + ix = ix - 360; + } + if (ix < 90) { return cosine[90 - ix] / 10; } + if (ix < 180) { return cosine[ix - 90] / 10; } + if (ix < 270) { return -cosine[270 - ix] / 10; } + if (ix < 360) { return -cosine[ix - 270] / 10; } + return 0; +} + +int cos_zelf(int ix) +{ + while (ix < 0) { + ix = ix + 360; + } + while (ix >= 360) { + ix = ix - 360; + } + if (ix < 90) { return cosine[ix] / 10; } + if (ix < 180) { return -cosine[180 - ix] / 10; } + if (ix < 270) { return -cosine[ix - 180] / 10; } + if (ix < 360) { return cosine[360 - ix] / 10; } + return 0; +} + +int tan_zelf(int ix) +{ + + while (ix < 0) { + ix = ix + 360; + } + while (ix >= 360) { + ix = ix - 360; + } + if (ix == 90) { return 9999; } + if (ix == 270) { return -9999; } + if (ix < 90) { return (1000 * cosine[90 - ix]) / cosine[ix]; } + if (ix < 180) { return -(1000 * cosine[ix - 90]) / cosine[180 - ix]; } + if (ix < 270) { return (1000 * cosine[270 - ix]) / cosine[ix - 180]; } + if (ix < 360) { return -(1000 * cosine[ix - 270]) / cosine[360 - ix]; } + return 0; +} + +int asin_zelf(int y, int hyp) +{ + int quot, sgn, ix; + if ((y > hyp) || (y == 0)) { + return 0; + } + sgn = hyp * y; + if (hyp < 0) { + hyp = -hyp; + } + if (y < 0) { + y = -y; + } + quot = (y * 10000) / hyp; + if (quot > 9999) { + quot = 9999; + } + for (ix = 0; ix < 90; ix++) + if ((quot < cosine[ix]) && (quot >= cosine[ix + 1])) { + break; + } + if (sgn < 0) { + return -(90 - ix); + } else { + return 90 - ix; + } +} + +int acos_zelf(int x, int hyp) +{ + int quot, sgn, ix; + if (x > hyp) { + return 0; + } + if (x == 0) { + if (hyp < 0) { + return -90; + } else { + return 90; + } + return 0; + } + sgn = hyp * x; + if (hyp < 0) { + hyp = -hyp; + } + if (x < 0) { + x = -x; + } + quot = (x * 10000) / hyp; + if (quot > 9999) { + quot = 9999; + } + for (ix = 0; ix < 90; ix++) + if ((quot < cosine[ix]) && (quot >= cosine[ix + 1])) { + break; + } + if (sgn < 0) { + return -ix; + } else { + return ix; + } +} + +//atan_zelf(y/x) in degrees +int atan_zelf(int y, int x) +{ + int angle, flip, t, xy; + + if (x < 0) { x = -x; } + if (y < 0) { y = -y; } + flip = 0; + if (x < y) { flip = 1; t = x; x = y; y = t; } + if (x == 0) { return 90; } + + xy = (y * 1000) / x; + angle = (360 * xy) / (6283 + ((((1764 * xy) / 1000) * xy) / 1000)); + if (flip) { angle = 90 - angle; } + return angle; +} + +unsigned int isqrt(unsigned int val) +{ + unsigned int temp, g = 0, b = 0x8000, bshft = 15; + do { + if (val >= (temp = (((g << 1) + b) << bshft--))) { + g += b; + val -= temp; + } + } while (b >>= 1); + return g; +} diff --git a/sw/airborne/modules/computer_vision/cv/trig.h b/sw/airborne/modules/computer_vision/cv/trig.h new file mode 100644 index 0000000000..9603d41789 --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv/trig.h @@ -0,0 +1,7 @@ +int cos_zelf(int ix); +int sin_zelf(int); +int tan_zelf(int); +int acos_zelf(int, int); +int asin_zelf(int, int); +int atan_zelf(int, int); +unsigned int isqrt(unsigned int); diff --git a/sw/airborne/modules/computer_vision/lib/v4l/video.h b/sw/airborne/modules/computer_vision/lib/v4l/video.h index 351ba68522..030f746d4b 100644 --- a/sw/airborne/modules/computer_vision/lib/v4l/video.h +++ b/sw/airborne/modules/computer_vision/lib/v4l/video.h @@ -22,7 +22,7 @@ #ifndef _VIDEO_H #define _VIDEO_H -#include "../../cv/image.h" +#include "modules/computer_vision/cv/image.h" struct buffer_struct { void *buf; diff --git a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c new file mode 100644 index 0000000000..8394ba32ec --- /dev/null +++ b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c @@ -0,0 +1,213 @@ +/* + * Copyright (C) 2014 Hann Woei Ho + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can 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, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file modules/computer_vision/opticflow/hover_stabilization.c + * @brief optical-flow based hovering for Parrot AR.Drone 2.0 + * + * Control loops for optic flow based hovering. + * Computes setpoint for the lower level attitude stabilization to control horizontal velocity. + */ + +// Own Header +#include "hover_stabilization.h" + +// Stabilization +#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h" +#include "firmwares/rotorcraft/guidance/guidance_v.h" +#include "autopilot.h" + +// Downlink +#include "subsystems/datalink/downlink.h" + +// Controller Gains +/* error if some gains are negative */ +#if (VISION_PHI_PGAIN < 0) || \ + (VISION_PHI_IGAIN < 0) || \ + (VISION_THETA_PGAIN < 0) || \ + (VISION_THETA_IGAIN < 0) +#error "ALL control gains have to be positive!!!" +#endif +bool activate_opticflow_hover; +float vision_desired_vx; +float vision_desired_vy; +int32_t vision_phi_pgain; +int32_t vision_phi_igain; +int32_t vision_theta_pgain; +int32_t vision_theta_igain; + +// Controller Commands +struct Int32Eulers cmd_euler; + +// Hover Stabilization +float Velx_Int; +float Vely_Int; +float Error_Velx; +float Error_Vely; + +#define CMD_OF_SAT 1500 // 40 deg = 2859.1851 +unsigned char saturateX = 0, saturateY = 0; +unsigned int set_heading; + + +#ifndef VISION_HOVER +#define VISION_HOVER TRUE +#endif + +#ifndef VISION_PHI_PGAIN +#define VISION_PHI_PGAIN 500. +#endif + +#ifndef VISION_PHI_IGAIN +#define VISION_PHI_IGAIN 10. +#endif + +#ifndef VISION_THETA_PGAIN +#define VISION_THETA_PGAIN 500. +#endif + +#ifndef VISION_THETA_IGAIN +#define VISION_THETA_IGAIN 10. +#endif + +#ifndef VISION_DESIRED_VX +#define VISION_DESIRED_VX 0. +#endif + +#ifndef VISION_DESIRED_VY +#define VISION_DESIRED_VY 0. +#endif + +void run_opticflow_hover(void); + +void guidance_h_module_enter(void) +{ + // INIT + Velx_Int = 0; + Vely_Int = 0; + // GUIDANCE: Set Hover-z-hold + guidance_v_z_sp = -1; +} + +void guidance_h_module_read_rc(void) +{ + // Do not read RC + // Setpoint being set by vision +} + +void guidance_h_module_run(bool_t in_flight) +{ + // Run + // Setpoint being set by vision + stabilization_attitude_run(in_flight); +} + + +void init_hover_stabilization_onvision() +{ + INT_EULERS_ZERO(cmd_euler); + + activate_opticflow_hover = VISION_HOVER; + vision_phi_pgain = VISION_PHI_PGAIN; + vision_phi_igain = VISION_PHI_IGAIN; + vision_theta_pgain = VISION_THETA_PGAIN; + vision_theta_igain = VISION_THETA_IGAIN; + vision_desired_vx = VISION_DESIRED_VX; + vision_desired_vy = VISION_DESIRED_VY; + + set_heading = 1; + + Error_Velx = 0; + Error_Vely = 0; + Velx_Int = 0; + Vely_Int = 0; +} + +void run_hover_stabilization_onvision(struct CVresults* vision ) +{ + struct FloatVect3 V_body; + if (activate_opticflow_hover == TRUE) { + // Compute body velocities from ENU + struct FloatVect3 *vel_ned = (struct FloatVect3*)stateGetSpeedNed_f(); + struct FloatQuat *q_n2b = stateGetNedToBodyQuat_f(); + float_quat_vmult(&V_body, q_n2b, vel_ned); + } + + // ************************************************************************************* + // Downlink Message + // ************************************************************************************* + + DOWNLINK_SEND_OF_HOVER(DefaultChannel, DefaultDevice, + &vision->FPS, &vision->dx_sum, &vision->dy_sum, &vision->OFx, &vision->OFy, + &vision->diff_roll, &vision->diff_pitch, + &vision->Velx, &vision->Vely, + &V_body.x, &V_body.y, + &vision->cam_h, &vision->count); + + if (autopilot_mode != AP_MODE_MODULE) { + return; + } + + if (vision->flow_count) { + Error_Velx = vision->Velx - vision_desired_vx; + Error_Vely = vision->Vely - vision_desired_vy; + } else { + Error_Velx = 0; + Error_Vely = 0; + } + + if (saturateX == 0) { + if (activate_opticflow_hover == TRUE) { + Velx_Int += vision_theta_igain * Error_Velx; + } else { + Velx_Int += vision_theta_igain * V_body.x; + } + } + if (saturateY == 0) { + if (activate_opticflow_hover == TRUE) { + Vely_Int += vision_phi_igain * Error_Vely; + } else { + Vely_Int += vision_phi_igain * V_body.y; + } + } + + if (set_heading) { + cmd_euler.psi = stateGetNedToBodyEulers_i()->psi; + set_heading = 0; + } + + if (activate_opticflow_hover == TRUE) { + cmd_euler.phi = - (vision_phi_pgain * Error_Vely + Vely_Int); + cmd_euler.theta = (vision_theta_pgain * Error_Velx + Velx_Int); + } else { + cmd_euler.phi = - (vision_phi_pgain * V_body.y + Vely_Int); + cmd_euler.theta = (vision_theta_pgain * V_body.x + Velx_Int); + } + + saturateX = 0; saturateY = 0; + if (cmd_euler.phi < -CMD_OF_SAT) {cmd_euler.phi = -CMD_OF_SAT; saturateX = 1;} + else if (cmd_euler.phi > CMD_OF_SAT) {cmd_euler.phi = CMD_OF_SAT; saturateX = 1;} + if (cmd_euler.theta < -CMD_OF_SAT) {cmd_euler.theta = -CMD_OF_SAT; saturateY = 1;} + else if (cmd_euler.theta > CMD_OF_SAT) {cmd_euler.theta = CMD_OF_SAT; saturateY = 1;} + + stabilization_attitude_set_rpy_setpoint_i(&cmd_euler); + DOWNLINK_SEND_VISION_STABILIZATION(DefaultChannel, DefaultDevice, &vision->Velx, &vision->Vely, &Velx_Int, + &Vely_Int, &cmd_euler.phi, &cmd_euler.theta); +} diff --git a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.h b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.h new file mode 100644 index 0000000000..285ac70864 --- /dev/null +++ b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.h @@ -0,0 +1,60 @@ +/* + * Copyright (C) 2014 Hann Woei Ho + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can 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, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file modules/computer_vision/opticflow/hover_stabilization.h + * @brief optical-flow based hovering for Parrot AR.Drone 2.0 + * + * Control loops for optic flow based hovering. + * Computes setpoint for the lower level attitude stabilization to control horizontal velocity. + */ + +#ifndef HOVER_STABILIZATION_H_ +#define HOVER_STABILIZATION_H_ + +#include +#include "inter_thread_data.h" + +// Controller module + +// Vertical loop re-uses Alt-hold +#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_HOVER + +// Horizontal mode is a specific controller +#define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_MODULE + +// Implement own Horizontal loops +extern void guidance_h_module_enter(void); +extern void guidance_h_module_read_rc(void); +extern void guidance_h_module_run(bool_t in_flight); + + +void init_hover_stabilization_onvision(void); +void run_hover_stabilization_onvision(struct CVresults *vision); + +extern bool activate_opticflow_hover; +extern float vision_desired_vx; +extern float vision_desired_vy; +extern int32_t vision_phi_pgain; +extern int32_t vision_phi_igain; +extern int32_t vision_theta_pgain; +extern int32_t vision_theta_igain; + +#endif /* HOVER_STABILIZATION_H_ */ diff --git a/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h b/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h new file mode 100644 index 0000000000..65ef0cb41e --- /dev/null +++ b/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h @@ -0,0 +1,56 @@ +/* + * Copyright (C) 2015 The Paparazzi Community + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can 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, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file modules/computer_vision/opticflow/inter_thread_data.h + * @brief Inter-thread data structures. + * + * Data structures used to for inter-thread communication via Unix Domain sockets. + */ + + +#ifndef _INTER_THREAD_DATA_H +#define _INTER_THREAD_DATA_H + +/// Data from thread to module +struct CVresults { + int cnt; // Number of processed frames + + float Velx; // Velocity as measured by camera + float Vely; + int flow_count; + + float cam_h; // Debug parameters + int count; + float OFx, OFy, dx_sum, dy_sum; + float diff_roll; + float diff_pitch; + float FPS; +}; + +/// Data from module to thread +struct PPRZinfo { + int cnt; // IMU msg counter + float phi; // roll [rad] + float theta; // pitch [rad] + float agl; // height above ground [m] +}; + +#endif diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c new file mode 100644 index 0000000000..0566279f1b --- /dev/null +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c @@ -0,0 +1,150 @@ +/* + * Copyright (C) 2015 The Paparazzi Community + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can 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, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file modules/computer_vision/opticflow/opticflow_thread.c + * + */ + +// Sockets +#include +#include +#include +#include + +#include "opticflow_thread.h" + + +///////////////////////////////////////////////////////////////////////// +// COMPUTER VISION THREAD + +// Video +#include "v4l/video.h" +#include "resize.h" + +// Payload Code +#include "visual_estimator.h" + +// Downlink Video +//#define DOWNLINK_VIDEO 1 + +#ifdef DOWNLINK_VIDEO +#include "encoding/jpeg.h" +#include "encoding/rtp.h" +#endif + +#include +#define DEBUG_INFO(X, ...) ; + +static volatile enum{RUN,EXIT} computer_vision_thread_command = RUN; /** request to close: set to 1 */ + +void computervision_thread_request_exit(void) { + computer_vision_thread_command = EXIT; +} + +void *computervision_thread_main(void *args) +{ + int thread_socket = *(int *) args; + + // Local data in/out + struct CVresults vision_results; + struct PPRZinfo autopilot_data; + + // Status + computer_vision_thread_command = RUN; + + // Video Input + struct vid_struct vid; + /* On ARDrone2: + * video1 = front camera; video2 = bottom camera + */ + vid.device = (char *)"/dev/video2"; + vid.w = 320; + vid.h = 240; + vid.n_buffers = 4; + + if (video_init(&vid) < 0) { + printf("Error initialising video\n"); + return 0; + } + + // Video Grabbing + struct img_struct *img_new = video_create_image(&vid); + +#ifdef DOWNLINK_VIDEO + // Video Compression + uint8_t *jpegbuf = (uint8_t *)malloc(vid.h * vid.w * 2); + + // Network Transmit + struct UdpSocket *vsock; + //#define FMS_UNICAST 0 + //#define FMS_BROADCAST 1 + vsock = udp_socket("192.168.1.255", 5000, 5001, FMS_BROADCAST); +#endif + + // First Apply Settings before init + opticflow_plugin_init(vid.w, vid.h, &vision_results); + + while (computer_vision_thread_command == RUN) { + + // Wait for a new frame + video_grab_image(&vid, img_new); + + // Get most recent State information + int bytes_read = sizeof(autopilot_data); + while (bytes_read == sizeof(autopilot_data)) + { + bytes_read = recv(thread_socket, &autopilot_data, sizeof(autopilot_data), MSG_DONTWAIT); + if (bytes_read != sizeof(autopilot_data)) { + if (bytes_read != -1) { + printf("[thread] Failed to read %d bytes PPRZ info from socket.\n",bytes_read); + } + } + } + DEBUG_INFO("[thread] Read # %d\n",autopilot_data.cnt); + + // Run Image Processing with image and data and get results + opticflow_plugin_run(img_new->buf, &autopilot_data, &vision_results); + + /* Send results to main */ + vision_results.cnt++; + int bytes_written = write(thread_socket, &vision_results, sizeof(vision_results)); + if (bytes_written != sizeof(vision_results)){ + perror("[thread] Failed to write to socket.\n"); + } + DEBUG_INFO("[thread] Write # %d, (bytes %d)\n",vision_results.cnt, bytes_written); + +#ifdef DOWNLINK_VIDEO + // JPEG encode the image: + uint32_t quality_factor = 10; //20 if no resize, + uint8_t dri_header = 0; + uint32_t image_format = FOUR_TWO_TWO; // format (in jpeg.h) + uint8_t *end = encode_image(small.buf, jpegbuf, quality_factor, image_format, small.w, small.h, dri_header); + uint32_t size = end - (jpegbuf); + + printf("Sending an image ...%u\n", size); + send_rtp_frame(vsock, jpegbuf, size, small.w, small.h, 0, quality_factor, dri_header, 0); +#endif + } + + printf("Thread Closed\n"); + video_close(&vid); + return 0; +} diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.h b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.h new file mode 100644 index 0000000000..5826da2229 --- /dev/null +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.h @@ -0,0 +1,33 @@ +/* + * Copyright (C) 2015 The Paparazzi Community + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can 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, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file modules/computer_vision/opticflow/opticflow_thread.h + * @brief computer vision thread + * + */ + +#ifndef OPTICFLOW_THREAD_H +#define OPTICFLOW_THREAD_H + +void *computervision_thread_main(void *args); /* computer vision thread: should be given a pointer to a socketpair as argument */ +void computervision_thread_request_exit(void); + +#endif diff --git a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c new file mode 100644 index 0000000000..48bd4ffd5d --- /dev/null +++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c @@ -0,0 +1,283 @@ +/* + * Copyright (C) 2014 Hann Woei Ho + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can 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, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file modules/computer_vision/opticflow/visual_estimator.c + * @brief Estimate velocity from optic flow. + * + * Using sensors from vertical camera and IMU of Parrot AR.Drone 2.0. + * + * Warning: all this code is called form the Vision-Thread: do not access any autopilot data in here. + */ + +#include "std.h" + +#include +#include +#include + +// Own Header +#include "visual_estimator.h" + +// Computer Vision +#include "opticflow/optic_flow_int.h" +#include "opticflow/fast9/fastRosten.h" + +// for FPS +#include "modules/computer_vision/cv/framerate.h" + + +// Local variables +struct visual_estimator_struct +{ + // Image size + unsigned int imgWidth; + unsigned int imgHeight; + + // Images + uint8_t *prev_frame; + uint8_t *gray_frame; + uint8_t *prev_gray_frame; + + // Initialization + int old_img_init; + + // Store previous + float prev_pitch; + float prev_roll; +} visual_estimator; + +// ARDrone Vertical Camera Parameters +#define FOV_H 0.67020643276 +#define FOV_W 0.89360857702 +#define Fx_ARdrone 343.1211 +#define Fy_ARdrone 348.5053 + +// Corner Detection +#define MAX_COUNT 100 + +// Flow Derotation +#define FLOW_DEROTATION + + +// Called by plugin +void opticflow_plugin_init(unsigned int w, unsigned int h, struct CVresults *results) +{ + // Initialize variables + visual_estimator.imgWidth = w; + visual_estimator.imgHeight = h; + + visual_estimator.gray_frame = (unsigned char *) calloc(w * h, sizeof(uint8_t)); + visual_estimator.prev_frame = (unsigned char *) calloc(w * h * 2, sizeof(uint8_t)); + visual_estimator.prev_gray_frame = (unsigned char *) calloc(w * h, sizeof(uint8_t)); + + visual_estimator.old_img_init = 1; + visual_estimator.prev_pitch = 0.0; + visual_estimator.prev_roll = 0.0; + + results->OFx = 0.0; + results->OFy = 0.0; + results->dx_sum = 0.0; + results->dy_sum = 0.0; + results->diff_roll = 0.0; + results->diff_pitch = 0.0; + results->cam_h = 0.0; + results->Velx = 0.0; + results->Vely = 0.0; + results->flow_count = 0; + results->cnt = 0; + results->count = 0; + + framerate_init(); +} + +void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CVresults *results) +{ + // Corner Tracking + // Working Variables + int max_count = 25; + int borderx = 24, bordery = 24; + int x[MAX_COUNT], y[MAX_COUNT]; + int new_x[MAX_COUNT], new_y[MAX_COUNT]; + int status[MAX_COUNT]; + int dx[MAX_COUNT], dy[MAX_COUNT]; + int w = visual_estimator.imgWidth; + int h = visual_estimator.imgHeight; + + // Framerate Measuring + results->FPS = framerate_run(); + + if (visual_estimator.old_img_init == 1) { + memcpy(visual_estimator.prev_frame, frame, w * h * 2); + CvtYUYV2Gray(visual_estimator.prev_gray_frame, visual_estimator.prev_frame, w, h); + visual_estimator.old_img_init = 0; + } + + // ************************************************************************************* + // Corner detection + // ************************************************************************************* + + // FAST corner detection + int fast_threshold = 20; + xyFAST *pnts_fast; + pnts_fast = fast9_detect((const byte *)visual_estimator.prev_gray_frame, w, h, w, + fast_threshold, &results->count); + if (results->count > MAX_COUNT) { results->count = MAX_COUNT; } + for (int i = 0; i < results->count; i++) { + x[i] = pnts_fast[i].x; + y[i] = pnts_fast[i].y; + } + free(pnts_fast); + + // Remove neighboring corners + const float min_distance = 3; + float min_distance2 = min_distance * min_distance; + int labelmin[MAX_COUNT]; + for (int i = 0; i < results->count; i++) { + for (int j = i + 1; j < results->count; j++) { + // distance squared: + float distance2 = (x[i] - x[j]) * (x[i] - x[j]) + (y[i] - y[j]) * (y[i] - y[j]); + if (distance2 < min_distance2) { + labelmin[i] = 1; + } + } + } + + int count_fil = results->count; + for (int i = results->count - 1; i >= 0; i--) { + int remove_point = 0; + + if (labelmin[i]) { + remove_point = 1; + } + + if (remove_point) { + for (int c = i; c < count_fil - 1; c++) { + x[c] = x[c + 1]; + y[c] = y[c + 1]; + } + count_fil--; + } + } + + if (count_fil > max_count) { count_fil = max_count; } + results->count = count_fil; + + // ************************************************************************************* + // Corner Tracking + // ************************************************************************************* + CvtYUYV2Gray(visual_estimator.gray_frame, frame, w, h); + + opticFlowLK(visual_estimator.gray_frame, visual_estimator.prev_gray_frame, x, y, + count_fil, w, h, new_x, new_y, status, 5, 100); + + results->flow_count = count_fil; + for (int i = count_fil - 1; i >= 0; i--) { + int remove_point = 1; + + if (status[i] && !(new_x[i] < borderx || new_x[i] > (w - 1 - borderx) || + new_y[i] < bordery || new_y[i] > (h - 1 - bordery))) { + remove_point = 0; + } + + if (remove_point) { + for (int c = i; c < results->flow_count - 1; c++) { + x[c] = x[c + 1]; + y[c] = y[c + 1]; + new_x[c] = new_x[c + 1]; + new_y[c] = new_y[c + 1]; + } + results->flow_count--; + } + } + + results->dx_sum = 0.0; + results->dy_sum = 0.0; + + // Optical Flow Computation + for (int i = 0; i < results->flow_count; i++) { + dx[i] = new_x[i] - x[i]; + dy[i] = new_y[i] - y[i]; + } + + // Median Filter + if (results->flow_count) { + quick_sort_int(dx, results->flow_count); // 11 + quick_sort_int(dy, results->flow_count); // 11 + + results->dx_sum = (float) dx[results->flow_count / 2]; + results->dy_sum = (float) dy[results->flow_count / 2]; + } else { + results->dx_sum = 0.0; + results->dy_sum = 0.0; + } + + // Flow Derotation + results->diff_pitch = (info->theta - visual_estimator.prev_pitch) * h / FOV_H; + results->diff_roll = (info->phi - visual_estimator.prev_roll) * w / FOV_W; + visual_estimator.prev_pitch = info->theta; + visual_estimator.prev_roll = info->phi; + + float OFx_trans, OFy_trans; +#ifdef FLOW_DEROTATION + if (results->flow_count) { + OFx_trans = results->dx_sum - results->diff_roll; + OFy_trans = results->dy_sum - results->diff_pitch; + + if ((OFx_trans <= 0) != (results->dx_sum <= 0)) { + OFx_trans = 0; + OFy_trans = 0; + } + } else { + OFx_trans = results->dx_sum; + OFy_trans = results->dy_sum; + } +#else + OFx_trans = results->dx_sum; + OFy_trans = results->dy_sum; +#endif + + // Average Filter + OFfilter(&results->OFx, &results->OFy, OFx_trans, OFy_trans, results->flow_count, 1); + + // Velocity Computation + if (info->agl < 0.01) { + results->cam_h = 0.01; + } + else { + results->cam_h = info->agl; + } + + if (results->flow_count) { + results->Velx = results->OFy * results->cam_h * results->FPS / Fy_ARdrone + 0.05; + results->Vely = -results->OFx * results->cam_h * results->FPS / Fx_ARdrone - 0.1; + } else { + results->Velx = 0.0; + results->Vely = 0.0; + } + + // ************************************************************************************* + // Next Loop Preparation + // ************************************************************************************* + + memcpy(visual_estimator.prev_frame, frame, w * h * 2); + memcpy(visual_estimator.prev_gray_frame, visual_estimator.gray_frame, w * h); + +} diff --git a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h new file mode 100644 index 0000000000..f499ffd320 --- /dev/null +++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2014 Hann Woei Ho + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can 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, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file modules/computer_vision/opticflow/visual_estimator.h + * @brief Estimate velocity from optic flow. + * + * Using sensors from vertical camera and IMU of Parrot AR.Drone 2.0 + */ + +#ifndef VISUAL_ESTIMATOR_H +#define VISUAL_ESTIMATOR_H + +#include "inter_thread_data.h" + +/** + * Initialize visual estimator. + * @param w image width + * @param h image height + */ +void opticflow_plugin_init(unsigned int w, unsigned int h, struct CVresults *results); +void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CVresults* results); + +#endif /* VISUAL_ESTIMATOR_H */ diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c new file mode 100644 index 0000000000..3687f65e2c --- /dev/null +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -0,0 +1,147 @@ +/* + * Copyright (C) 2014 Hann Woei Ho + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can 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, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file modules/computer_vision/opticflow_module.c + * @brief optical-flow based hovering for Parrot AR.Drone 2.0 + * + * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 + */ + + +#include "opticflow_module.h" + +// Computervision Runs in a thread +#include "opticflow/opticflow_thread.h" +#include "opticflow/inter_thread_data.h" + +// Navigate Based On Vision, needed to call init/run_hover_stabilization_onvision +#include "opticflow/hover_stabilization.h" + +// Threaded computer vision +#include + +// Sockets +#include +#include +#include +#include +#include + +int cv_sockets[2]; + +// Paparazzi Data +#include "state.h" +#include "subsystems/abi.h" + +// Downlink +#include "subsystems/datalink/downlink.h" + + +struct PPRZinfo opticflow_module_data; + +/** height above ground level, from ABI + * Used for scale computation, negative value means invalid. + */ +/** default sonar/agl to use in opticflow visual_estimator */ +#ifndef OPTICFLOW_AGL_ID +#define OPTICFLOW_AGL_ID ABI_BROADCAST +#endif +abi_event agl_ev; +static void agl_cb(uint8_t sender_id, float distance); + +static void agl_cb(uint8_t sender_id __attribute__((unused)), float distance) +{ + if (distance > 0) { + opticflow_module_data.agl = distance; + } +} + +#define DEBUG_INFO(X, ...) ; + +void opticflow_module_init(void) +{ + // get AGL from sonar via ABI + AbiBindMsgAGL(OPTICFLOW_AGL_ID, &agl_ev, agl_cb); + + // Initialize local data + opticflow_module_data.cnt = 0; + opticflow_module_data.phi = 0; + opticflow_module_data.theta = 0; + opticflow_module_data.agl = 0; + + // Stabilization Code Initialization + init_hover_stabilization_onvision(); +} + + +void opticflow_module_run(void) +{ + // Send Updated data to thread + opticflow_module_data.cnt++; + opticflow_module_data.phi = stateGetNedToBodyEulers_f()->phi; + opticflow_module_data.theta = stateGetNedToBodyEulers_f()->theta; + int bytes_written = write(cv_sockets[0], &opticflow_module_data, sizeof(opticflow_module_data)); + if (bytes_written != sizeof(opticflow_module_data)){ + printf("[module] Failed to write to socket: written = %d, error=%d.\n",bytes_written, errno); + } + else { + DEBUG_INFO("[module] Write # %d (%d bytes)\n",opticflow_module_data.cnt, bytes_written); + } + + // Read Latest Vision Module Results + struct CVresults vision_results; + // Warning: if the vision runs faster than the module, you need to read multiple times + int bytes_read = recv(cv_sockets[0], &vision_results, sizeof(vision_results), MSG_DONTWAIT); + if (bytes_read != sizeof(vision_results)) { + if (bytes_read != -1) { + printf("[module] Failed to read %d bytes: CV results from socket errno=%d.\n",bytes_read, errno); + } + } else { + //////////////////////////////////////////// + // Module-Side Code + //////////////////////////////////////////// + DEBUG_INFO("[module] Read vision %d\n",vision_results.cnt); + run_hover_stabilization_onvision(&vision_results); + } +} + +void opticflow_module_start(void) +{ + pthread_t computervision_thread; + if (socketpair(AF_UNIX, SOCK_DGRAM, 0, cv_sockets) == 0) { + //////////////////////////////////////////// + // Thread-Side Code + //////////////////////////////////////////// + int rc = pthread_create(&computervision_thread, NULL, computervision_thread_main, + &cv_sockets[1]); + if (rc) { + printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc); + } + } + else { + perror("Could not create socket.\n"); + } +} + +void opticflow_module_stop(void) +{ + computervision_thread_request_exit(); +} diff --git a/sw/airborne/modules/computer_vision/opticflow_module.h b/sw/airborne/modules/computer_vision/opticflow_module.h new file mode 100644 index 0000000000..098d0b15b3 --- /dev/null +++ b/sw/airborne/modules/computer_vision/opticflow_module.h @@ -0,0 +1,39 @@ +/* + * Copyright (C) 2014 Hann Woei Ho + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can 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, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file modules/computer_vision/opticflow_module.h + * @brief optical-flow based hovering for Parrot AR.Drone 2.0 + * + * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 + */ + +#ifndef OPTICFLOW_MODULE_H +#define OPTICFLOW_MODULE_H + +#include "std.h" + +// Module functions +extern void opticflow_module_init(void); +extern void opticflow_module_run(void); +extern void opticflow_module_start(void); +extern void opticflow_module_stop(void); + +#endif /* OPTICFLOW_MODULE_H */ diff --git a/sw/airborne/modules/computer_vision/video_usb_logger.c b/sw/airborne/modules/computer_vision/video_usb_logger.c new file mode 100644 index 0000000000..1306fb8ea3 --- /dev/null +++ b/sw/airborne/modules/computer_vision/video_usb_logger.c @@ -0,0 +1,88 @@ +/* + * Copyright (C) 2015 The Paparazzi Community + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can 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, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +/** @file modules/computer_vision/video_usb_logger.c + */ + +#include "video_usb_logger.h" + +#include +#include "state.h" +#include "viewvideo.h" + +/** Set the default File logger path to the USB drive */ +#ifndef VIDEO_USB_LOGGER_PATH +#define VIDEO_USB_LOGGER_PATH "/data/video/usb/" +#endif + +/** The file pointer */ +static FILE *video_usb_logger = NULL; + +/** Start the file logger and open a new file */ +void video_usb_logger_start(void) +{ + uint32_t counter = 0; + char filename[512]; + + // Check for available files + sprintf(filename, "%s%05d.csv", VIDEO_USB_LOGGER_PATH, counter); + while ((video_usb_logger = fopen(filename, "r"))) { + fclose(video_usb_logger); + + counter++; + sprintf(filename, "%s%05d.csv", VIDEO_USB_LOGGER_PATH, counter); + } + + video_usb_logger = fopen(filename, "w"); + + if (video_usb_logger != NULL) { + fprintf(video_usb_logger, "counter,image,roll,pitch,yaw,x,y,z,sonar\n"); + } +} + +/** Stop the logger an nicely close the file */ +void video_usb_logger_stop(void) +{ + if (video_usb_logger != NULL) { + fclose(video_usb_logger); + video_usb_logger = NULL; + } +} + +/** Log the values to a csv file */ +void video_usb_logger_periodic(void) +{ + if (video_usb_logger == NULL) { + return; + } + static uint32_t counter = 0; + struct NedCoor_i *ned = stateGetPositionNed_i(); + struct Int32Eulers *euler = stateGetNedToBodyEulers_i(); + static uint32_t sonar = 0; + + fprintf(video_usb_logger, "%d,%d,%d,%d,%d,%d,%d,%d,%d\n", counter, + viewvideo_save_shot_number, euler->phi, euler->theta, euler->psi, ned->x, + ned->y, ned->z, sonar); + counter++; + // Save a new shot + viewvideo_SaveShot(0); +} diff --git a/sw/airborne/modules/computer_vision/video_usb_logger.h b/sw/airborne/modules/computer_vision/video_usb_logger.h new file mode 100644 index 0000000000..d54dab5d36 --- /dev/null +++ b/sw/airborne/modules/computer_vision/video_usb_logger.h @@ -0,0 +1,34 @@ +/* + * Copyright (C) 2015 The Paparazzi Community + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can 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, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +/** @file modules/computer_vission/video_usb_logger.h + * @brief Camera image logger for Linux based autopilots + */ + +#ifndef VIDEO_USB_LOGGER_H_ +#define VIDEO_USB_LOGGER_H_ + +extern void video_usb_logger_start(void); +extern void video_usb_logger_stop(void); +extern void video_usb_logger_periodic(void); + +#endif /* VIDEO_USB_LOGGER_H_ */ diff --git a/sw/airborne/modules/computer_vision/viewvideo.c b/sw/airborne/modules/computer_vision/viewvideo.c index 7a5dfd0a95..0e4e55fa4e 100644 --- a/sw/airborne/modules/computer_vision/viewvideo.c +++ b/sw/airborne/modules/computer_vision/viewvideo.c @@ -85,6 +85,8 @@ void viewvideo_run(void) {} // take shot flag int viewvideo_shot = 0; +volatile int viewvideo_save_shot_number = 0; + ///////////////////////////////////////////////////////////////////////// // COMPUTER VISION THREAD @@ -97,9 +99,15 @@ void *computervision_thread_main(void *data) { // Video Input struct vid_struct vid; +#if USE_BOTTOM_CAMERA + vid.device = (char *)"/dev/video2"; + vid.w = 320; + vid.h = 240; +#else vid.device = (char *)"/dev/video1"; vid.w = 1280; vid.h = 720; +#endif vid.n_buffers = 4; if (video_init(&vid) < 0) { printf("Error initialising video\n"); @@ -148,11 +156,11 @@ void *computervision_thread_main(void *data) while (computer_vision_thread_command > 0) { // compute usleep to have a more stable frame rate - struct timeval time; - gettimeofday(&time, NULL); - int dt = (int)(time.tv_sec - last_time.tv_sec) * 1000000 + (int)(time.tv_usec - last_time.tv_usec); + struct timeval vision_thread_sleep_time; + gettimeofday(&vision_thread_sleep_time, NULL); + int dt = (int)(vision_thread_sleep_time.tv_sec - last_time.tv_sec) * 1000000 + (int)(vision_thread_sleep_time.tv_usec - last_time.tv_usec); if (dt < microsleep) { usleep(microsleep - dt); } - last_time = time; + last_time = vision_thread_sleep_time; // Grab new frame video_grab_image(&vid, img_new); @@ -163,16 +171,26 @@ void *computervision_thread_main(void *data) uint32_t size = end - (jpegbuf); FILE *save; char save_name[128]; +#if LOG_ON_USB + if (system("mkdir -p /data/video/usb/images") == 0) { +#else if (system("mkdir -p /data/video/images") == 0) { +#endif // search available index (max is 99) - for (; file_index < 99; file_index++) { + for (; file_index < 99999; file_index++) { printf("search %d\n", file_index); - sprintf(save_name, "/data/video/images/img_%02d.jpg", file_index); +#if LOG_ON_USB + sprintf(save_name, "/data/video/usb/images/img_%05d.jpg", file_index); +#else + sprintf(save_name, "/data/video/images/img_%05d.jpg", file_index); +#endif // test if file exists or not if (access(save_name, F_OK) == -1) { printf("access\n"); save = fopen(save_name, "w"); if (save != NULL) { + // Atomic copy + viewvideo_save_shot_number = file_index; fwrite(jpegbuf, sizeof(uint8_t), size, save); fclose(save); } else { @@ -181,11 +199,12 @@ void *computervision_thread_main(void *data) // leave for loop break; } else { - printf("file exists\n"); + //printf("file exists\n"); } } } - computer_vision_thread_command = 1; + if (computer_vision_thread_command == 2) + computer_vision_thread_command = 1; viewvideo_shot = 0; } @@ -206,7 +225,7 @@ void *computervision_thread_main(void *data) 0, // Format 422 quality_factor, // Jpeg-Quality dri_jpeg_header, // DRI Header - 1 // 90kHz time increment + 0 // 90kHz time increment ); // Extra note: when the time increment is set to 0, // it is automaticaly calculated by the send_rtp_frame function diff --git a/sw/airborne/modules/computer_vision/viewvideo.h b/sw/airborne/modules/computer_vision/viewvideo.h index d04b07af1a..a4177815e9 100644 --- a/sw/airborne/modules/computer_vision/viewvideo.h +++ b/sw/airborne/modules/computer_vision/viewvideo.h @@ -39,6 +39,7 @@ extern void viewvideo_stop(void); // Save picture on disk at full resolution // can be called from flight plan extern int viewvideo_save_shot(void); +extern volatile int viewvideo_save_shot_number; extern int viewvideo_shot; #define viewvideo_SaveShot(_v) { \ diff --git a/sw/airborne/modules/ctrl/ctrl_module_demo.c b/sw/airborne/modules/ctrl/ctrl_module_demo.c new file mode 100644 index 0000000000..0e3182467e --- /dev/null +++ b/sw/airborne/modules/ctrl/ctrl_module_demo.c @@ -0,0 +1,104 @@ +/* + * Copyright (C) 2015 + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can 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, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file modules/ctrl/ctrl_module_demo.h + * @brief example empty controller + * + */ + +#include "modules/ctrl/ctrl_module_demo.h" +#include "state.h" +#include "subsystems/radio_control.h" +#include "firmwares/rotorcraft/stabilization.h" + +struct ctrl_module_demo_struct { + int rc_x; + int rc_y; + int rc_z; + int rc_t; + +} ctrl_module_demo; + +float ctrl_module_demo_pr_ff_gain = 0.2f; // Pitch/Roll +float ctrl_module_demo_pr_d_gain = 0.1f; +float ctrl_module_demo_y_ff_gain = 0.4f; // Yaw +float ctrl_module_demo_y_d_gain = 0.05f; + +void ctrl_module_init(void); +void ctrl_module_run(bool_t in_flight); + +void ctrl_module_init(void) +{ + ctrl_module_demo.rc_x = 0; + ctrl_module_demo.rc_y = 0; + ctrl_module_demo.rc_z = 0; + ctrl_module_demo.rc_t = 0; +} + +// Old-fashened rate control without reference model nor attitude +void ctrl_module_run(bool_t in_flight) +{ + if (!in_flight) { + // Reset integrators + stabilization_cmd[COMMAND_ROLL] = 0; + stabilization_cmd[COMMAND_PITCH] = 0; + stabilization_cmd[COMMAND_YAW] = 0; + stabilization_cmd[COMMAND_THRUST] = 0; + } else { + stabilization_cmd[COMMAND_ROLL] = ctrl_module_demo.rc_x * ctrl_module_demo_pr_ff_gain - stateGetBodyRates_i()->p * + ctrl_module_demo_pr_d_gain; + stabilization_cmd[COMMAND_PITCH] = ctrl_module_demo.rc_y * ctrl_module_demo_pr_ff_gain - stateGetBodyRates_i()->q * + ctrl_module_demo_pr_d_gain; + stabilization_cmd[COMMAND_YAW] = ctrl_module_demo.rc_z * ctrl_module_demo_y_ff_gain - stateGetBodyRates_i()->r * + ctrl_module_demo_y_d_gain; + stabilization_cmd[COMMAND_THRUST] = ctrl_module_demo.rc_t; + } +} + + + +//////////////////////////////////////////////////////////////////// +// Call our controller +// Implement own Horizontal loops +void guidance_h_module_enter(void) +{ + ctrl_module_init(); +} +void guidance_h_module_read_rc(void) +{ + // -MAX_PPRZ to MAX_PPRZ + ctrl_module_demo.rc_t = radio_control.values[RADIO_THROTTLE]; + ctrl_module_demo.rc_x = radio_control.values[RADIO_ROLL]; + ctrl_module_demo.rc_y = radio_control.values[RADIO_PITCH]; + ctrl_module_demo.rc_z = radio_control.values[RADIO_YAW]; +} + +void guidance_h_module_run(bool_t in_flight) +{ + // Call full inner-/outerloop / horizontal-/vertical controller: + ctrl_module_run(in_flight); +} + +// Implement own Horizontal loops +inline void guidance_v_module_enter(void) {} +inline void guidance_v_module_run(UNUSED bool_t in_flight) {} + + diff --git a/sw/airborne/modules/ctrl/ctrl_module_demo.h b/sw/airborne/modules/ctrl/ctrl_module_demo.h new file mode 100644 index 0000000000..07a99eef7b --- /dev/null +++ b/sw/airborne/modules/ctrl/ctrl_module_demo.h @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2015 + * + * This file is part of Paparazzi. + * + * Paparazzi is free software; you can 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, or (at your option) + * any later version. + * + * Paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file modules/ctrl/ctrl_module_demo.c + * @brief example empty controller + * + */ + +#ifndef CTRL_MODULE_DEMO_H_ +#define CTRL_MODULE_DEMO_H_ + +#include + +// Settings +extern float ctrl_module_demo_pr_ff_gain; // Pitch/Roll +extern float ctrl_module_demo_pr_d_gain; +extern float ctrl_module_demo_y_ff_gain; // Yaw +extern float ctrl_module_demo_y_d_gain; + + +// Demo with own guidance_h +#define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_MODULE + +// and own guidance_v +#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_MODULE + +// Implement own Horizontal loops +extern void guidance_h_module_enter(void); +extern void guidance_h_module_read_rc(void); +extern void guidance_h_module_run(bool_t in_flight); + +// Implement own Horizontal loops +extern void guidance_v_module_enter(void); +extern void guidance_v_module_run(bool_t in_flight); + + +#endif /* HOVER_STABILIZATION_H_ */ diff --git a/sw/airborne/modules/datalink/extra_pprz_dl.c b/sw/airborne/modules/datalink/extra_pprz_dl.c index cb3c85c2d4..20693a2d9d 100644 --- a/sw/airborne/modules/datalink/extra_pprz_dl.c +++ b/sw/airborne/modules/datalink/extra_pprz_dl.c @@ -21,11 +21,12 @@ * */ -#include "extra_pprz_dl.h" - -#if DATALINK != PPRZ -uint8_t ck_a, ck_b; -#endif +#include "modules/datalink/extra_pprz_dl.h" struct pprz_transport extra_pprz_tp; +void extra_pprz_dl_init(void) +{ + pprz_transport_init(&extra_pprz_tp); +} + diff --git a/sw/airborne/modules/datalink/extra_pprz_dl.h b/sw/airborne/modules/datalink/extra_pprz_dl.h index 8e278c7fe9..08e5a34497 100644 --- a/sw/airborne/modules/datalink/extra_pprz_dl.h +++ b/sw/airborne/modules/datalink/extra_pprz_dl.h @@ -41,6 +41,8 @@ extern struct pprz_transport extra_pprz_tp; DlCheckAndParse(); \ } +/** Init function */ +extern void extra_pprz_dl_init(void); #endif /* EXTRA_PPRZ_DL_H */ diff --git a/sw/airborne/modules/loggers/file_logger.c b/sw/airborne/modules/loggers/file_logger.c index 94fcc36ebe..31bf528e27 100644 --- a/sw/airborne/modules/loggers/file_logger.c +++ b/sw/airborne/modules/loggers/file_logger.c @@ -37,7 +37,7 @@ #endif /** The file pointer */ -static FILE *file_logger; +static FILE *file_logger = NULL; /** Start the file logger and open a new file */ void file_logger_start(void) @@ -67,8 +67,10 @@ void file_logger_start(void) /** Stop the logger an nicely close the file */ void file_logger_stop(void) { - fclose(file_logger); - file_logger = NULL; + if (file_logger != NULL) { + fclose(file_logger); + file_logger = NULL; + } } /** Log the values to a csv file */ diff --git a/sw/airborne/modules/meteo/meteo_france_DAQ.c b/sw/airborne/modules/meteo/meteo_france_DAQ.c index 3ea2c403f7..499c926257 100644 --- a/sw/airborne/modules/meteo/meteo_france_DAQ.c +++ b/sw/airborne/modules/meteo/meteo_france_DAQ.c @@ -68,7 +68,7 @@ void init_mf_daq(void) void mf_daq_send_state(void) { // Send aircraft state to DAQ board - DOWNLINK_SEND_MF_DAQ_STATE(pprz_tp, EXTRA_DOWNLINK_DEVICE, + DOWNLINK_SEND_MF_DAQ_STATE(extra_pprz_tp, EXTRA_DOWNLINK_DEVICE, &autopilot_flight_time, &stateGetBodyRates_f()->p, &stateGetBodyRates_f()->q, diff --git a/sw/airborne/modules/meteo/meteo_stick.c b/sw/airborne/modules/meteo/meteo_stick.c index e19c51963a..2f0accd1f6 100644 --- a/sw/airborne/modules/meteo/meteo_stick.c +++ b/sw/airborne/modules/meteo/meteo_stick.c @@ -183,14 +183,14 @@ void meteo_stick_event(void) // send absolute pressure data over ABI as soon as available if (meteo_stick.pressure.data_available) { float abs = MS_PRESSURE_SCALE * (float)((int32_t)meteo_stick.pressure.data - MS_PRESSURE_OFFSET); - AbiSendMsgBARO_ABS(METEO_STICK_SENDER_ID, &abs); + AbiSendMsgBARO_ABS(METEO_STICK_SENDER_ID, abs); meteo_stick.pressure.data_available = FALSE; } // send differential pressure data over ABI as soon as available if (meteo_stick.diff_pressure.data_available) { float diff = MS_DIFF_PRESSURE_SCALE * (float)((int32_t)meteo_stick.diff_pressure.data - MS_DIFF_PRESSURE_OFFSET); - AbiSendMsgBARO_DIFF(METEO_STICK_SENDER_ID, &diff); + AbiSendMsgBARO_DIFF(METEO_STICK_SENDER_ID, diff); meteo_stick.diff_pressure.data_available = FALSE; } } diff --git a/sw/airborne/modules/multi/follow.c b/sw/airborne/modules/multi/follow.c index 0c07968e9a..68b5397213 100644 --- a/sw/airborne/modules/multi/follow.c +++ b/sw/airborne/modules/multi/follow.c @@ -30,7 +30,6 @@ #include "generated/airframe.h" #include "state.h" -#include "subsystems/ins/ins_int.h" #include "navigation.h" #include "messages.h" #include "dl_protocol.h" @@ -61,7 +60,7 @@ void follow_change_wp(unsigned char *buffer) new_pos.z = DL_REMOTE_GPS_ecef_z(buffer); // Translate to ENU - enu_of_ecef_point_i(&enu, &ins_impl.ltp_def, &new_pos); + enu_of_ecef_point_i(&enu, &state.ned_origin_i, &new_pos); INT32_VECT3_SCALE_2(enu, enu, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); // TODO: Add the angle to the north diff --git a/sw/airborne/modules/nav/nav_bungee_takeoff.c b/sw/airborne/modules/nav/nav_bungee_takeoff.c index 6d0ff21fa7..f68f8a64e2 100644 --- a/sw/airborne/modules/nav/nav_bungee_takeoff.c +++ b/sw/airborne/modules/nav/nav_bungee_takeoff.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2008-2013 The Paparazzi Team + * Copyright (C) 2008-2015 The Paparazzi Team * * This file is part of paparazzi. * @@ -14,206 +14,199 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . */ /** * @file modules/nav/nav_bungee_takeoff.c * - * from OSAM advanced navigation routines + * Takeoff functions for bungee takeoff. + * + * Run initialize function when the plane is on the bungee, the bungee is + * fully extended and you are ready to launch the plane. + * After initialized, the plane will follow a line drawn by the position + * of the plane on initialization and the position of the bungee (given in + * the arguments). + * Once the plane crosses the throttle line, which is perpendicular to the line + * the plane is following, and intersects the position of the bungee (plus or + * minus a fixed distance (BUNGEE_TAKEOFF_DISTANCE in airframe file) from + * the bungee just in case the bungee doesn't release exactly above the bungee) + * the prop will come on. + * The plane will then continue to follow the line until it has reached a + * specific height (defined in as BUNGEE_TAKEOFF_HEIGHT in airframe file) above + * the bungee waypoint and airspeed (defined as BUNGEE_TAKEOFF_AIRSPEED in the + * airframe file). The airspeed limit is only used if USE_AIRSPEED flag is + * defined or set to true (and assuming the airspeed is then available). + * It is also possible to specify the pitch angle (BUNGEE_TAKEOFF_PITCH) and + * the throttle (BUNGEE_TAKEOFF_THROTTLE, between 0 and 1). + * + * @verbatim + *
+ * + * + * + * + * + * + *
+ * @endverbatim + * + * + * initial code from OSAM advanced navigation routines * */ #include "modules/nav/nav_bungee_takeoff.h" -#include "firmwares/fixedwing/nav.h" #include "state.h" -#include "autopilot.h" +#include "paparazzi.h" +#include "firmwares/fixedwing/nav.h" +#include "firmwares/fixedwing/autopilot.h" #include "generated/flight_plan.h" +#include "generated/airframe.h" +#include "math/pprz_algebra_float.h" -/************** Bungee Takeoff **********************************************/ -/** Takeoff functions for bungee takeoff. - Run initialize function when the plane is on the bungee, the bungee is fully extended and you are ready to - launch the plane. After initialized, the plane will follow a line drawn by the position of the plane on initialization and the - position of the bungee (given in the arguments). Once the plane crosses the throttle line, which is perpendicular to the line the plane is following, - and intersects the position of the bungee (plus or minus a fixed distance (TakeOff_Distance in airframe file) from the bungee just in case the bungee doesn't release directly above the bungee) the prop will come on. The plane will then continue to follow the line until it has reached a specific - height (defined in as Takeoff_Height in airframe file) above the bungee waypoint and speed (defined as Takeoff_Speed in the airframe file). - @verbatim -
- - - - -
- @endverbatim -*/ - -#ifndef Takeoff_Distance -#define Takeoff_Distance 10 +// Backward compatibility +#ifdef Takeoff_Distance +#warning "Takeoff_Distance depreciated, please use BUNGEE_TAKEOFF_DISTANCE instead" +#define BUNGEE_TAKEOFF_DISTANCE Takeoff_Distance #endif -#ifndef Takeoff_Height -#define Takeoff_Height 30 +#ifdef Takeoff_Height +#warning "Takeoff_Height depreciated, please use BUNGEE_TAKEOFF_HEIGHT instead" +#define BUNGEE_TAKEOFF_HEIGHT Takeoff_Height #endif -#ifndef Takeoff_Speed -#define Takeoff_Speed 15 +#ifdef Takeoff_Speed +#warning "Takeoff_Speed depreciated, please use BUNGEE_TAKEOFF_AIRSPEED instead (beware that USE_AIRSPEED flag is needed)" +#define BUNGEE_TAKEOFF_AIRSPEED Takeoff_Speed #endif -#ifndef Takeoff_MinSpeed -#define Takeoff_MinSpeed 5 +#ifdef Takeoff_MinSpeed +#warning "Takeoff_MinSpeed depreciated, please use BUNGEE_TAKEOFF_MIN_SPEED instead" +#define BUNGEE_TAKEOFF_MIN_SPEED Takeoff_MinSpeed #endif -enum TakeoffStatus { Launch, Throttle, Finished }; + +#ifndef BUNGEE_TAKEOFF_DISTANCE +#define BUNGEE_TAKEOFF_DISTANCE 10.0 +#endif +#ifndef BUNGEE_TAKEOFF_HEIGHT +#define BUNGEE_TAKEOFF_HEIGHT 30.0 +#endif +#if USE_AIRSPEED +#ifndef BUNGEE_TAKEOFF_AIRSPEED +#define BUNGEE_TAKEOFF_AIRSPEED 15.0 +#endif +#else +#ifdef BUNGEE_TAKEOFF_AIRSPEED +#warning "BUNGEE_TAKEOFF_AIRSPEED is defined but not USE_AIRSPEED. Airspeed limit will not be used" +#endif +#endif +#ifndef BUNGEE_TAKEOFF_MIN_SPEED +#define BUNGEE_TAKEOFF_MIN_SPEED 5.0 +#endif +#ifndef BUNGEE_TAKEOFF_THROTTLE +#define BUNGEE_TAKEOFF_THROTTLE 1.0 +#endif +#ifndef BUNGEE_TAKEOFF_PITCH +#ifdef AGR_CLIMB_PITCH +#define BUNGEE_TAKEOFF_PITCH AGR_CLIMB_PITCH +#else +#define BUNGEE_TAKEOFF_PITCH RadOfDeg(15.) +#endif +#endif + +enum TakeoffStatus { + Launch, + Throttle, + Finished +}; + static enum TakeoffStatus CTakeoffStatus; -static float throttlePx; -static float throttlePy; -static float initialx; -static float initialy; -static float ThrottleSlope; -static bool_t AboveLine; -static float BungeeAlt; -static float TDistance; -static uint8_t BungeeWaypoint; -bool_t nav_bungee_takeoff_setup(uint8_t BungeeWP) +static struct FloatVect2 init_point; +static struct FloatVect2 throttle_point; +static struct FloatVect2 takeoff_dir; +static struct FloatVect3 bungee_point; + +static void compute_points_from_bungee(void) { - float ThrottleB; + // Store init point (current position, where the plane will be released) + VECT2_ASSIGN(init_point, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y); + // Compute unitary 2D vector (bungee_point - init_point) = takeoff direction + VECT2_DIFF(takeoff_dir, bungee_point, init_point); + float_vect2_normalize(&takeoff_dir); + // Find throttle point (the point where the throttle line and launch line intersect) + // If TakeOff_Distance is positive, throttle point is after bungee point, before otherwise + VECT2_SMUL(throttle_point, takeoff_dir, BUNGEE_TAKEOFF_DISTANCE); + VECT2_SUM(throttle_point, bungee_point, throttle_point); +} - initialx = stateGetPositionEnu_f()->x; - initialy = stateGetPositionEnu_f()->y; +bool_t nav_bungee_takeoff_setup(uint8_t bungee_wp) +{ + // Store bungee point (from WP id, altitude should be ground alt) + // FIXME use current alt instead ? + VECT3_ASSIGN(bungee_point, WaypointX(bungee_wp), WaypointY(bungee_wp), WaypointAlt(bungee_wp)); - BungeeWaypoint = BungeeWP; + // Compute other points + compute_points_from_bungee(); - //Takeoff_Distance can only be positive - TDistance = fabs(Takeoff_Distance); - - //Translate initial position so that the position of the bungee is (0,0) - float Currentx = initialx - (WaypointX(BungeeWaypoint)); - float Currenty = initialy - (WaypointY(BungeeWaypoint)); - - //Record bungee alt (which should be the ground alt at that point) - BungeeAlt = waypoints[BungeeWaypoint].a; - - //Find Launch line slope and Throttle line slope - float MLaunch = Currenty / Currentx; - - //Find Throttle Point (the point where the throttle line and launch line intersect) - if (Currentx < 0) { - throttlePx = TDistance / sqrt(MLaunch * MLaunch + 1); - } else { - throttlePx = -(TDistance / sqrt(MLaunch * MLaunch + 1)); - } - - if (Currenty < 0) { - throttlePy = sqrt((TDistance * TDistance) - (throttlePx * throttlePx)); - } else { - throttlePy = -sqrt((TDistance * TDistance) - (throttlePx * throttlePx)); - } - - //Find ThrottleLine - ThrottleSlope = tan(atan2(Currenty, Currentx) + (3.14 / 2)); - ThrottleB = (throttlePy - (ThrottleSlope * throttlePx)); - - //Determine whether the UAV is below or above the throttle line - if (Currenty > ((ThrottleSlope * Currentx) + ThrottleB)) { - AboveLine = TRUE; - } else { - AboveLine = FALSE; - } - - //Enable Launch Status and turn kill throttle on + // Enable Launch Status and turn kill throttle on CTakeoffStatus = Launch; kill_throttle = 1; - //Translate the throttle point back - throttlePx = throttlePx + (WaypointX(BungeeWP)); - throttlePy = throttlePy + (WaypointY(BungeeWP)); - return FALSE; } bool_t nav_bungee_takeoff_run(void) { - //Translate current position so Throttle point is (0,0) - float Currentx = stateGetPositionEnu_f()->x - throttlePx; - float Currenty = stateGetPositionEnu_f()->y - throttlePy; - bool_t CurrentAboveLine; - float ThrottleB; + float cross = 0.; + + // Get current position + struct FloatVect2 pos; + VECT2_ASSIGN(pos, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y); switch (CTakeoffStatus) { case Launch: - //Follow Launch Line - NavVerticalAutoThrottleMode(0); - NavVerticalAltitudeMode(BungeeAlt + Takeoff_Height, 0.); - nav_route_xy(initialx, initialy, throttlePx, throttlePy); + // Recalculate lines if below min speed + if ((*stateGetHorizontalSpeedNorm_f()) < BUNGEE_TAKEOFF_MIN_SPEED) { + compute_points_from_bungee(); + } + + // Follow Launch Line with takeoff pitch and no throttle + NavVerticalAutoThrottleMode(BUNGEE_TAKEOFF_PITCH); + NavVerticalThrottleMode(0); + // FIXME previously using altitude mode, maybe not wise without motors + //NavVerticalAltitudeMode(bungee_point.z + BUNGEE_TAKEOFF_HEIGHT, 0.); + nav_route_xy(init_point.x, init_point.y, throttle_point.x, throttle_point.y); kill_throttle = 1; - //recalculate lines if below min speed - if ((*stateGetHorizontalSpeedNorm_f()) < Takeoff_MinSpeed) { - initialx = stateGetPositionEnu_f()->x; - initialy = stateGetPositionEnu_f()->y; + // Find out if UAV has crossed the line + VECT2_DIFF(pos, pos, throttle_point); // position local to throttle_point + cross = VECT2_DOT_PRODUCT(pos, takeoff_dir); - //Translate initial position so that the position of the bungee is (0,0) - Currentx = initialx - (WaypointX(BungeeWaypoint)); - Currenty = initialy - (WaypointY(BungeeWaypoint)); - - //Find Launch line slope - float MLaunch = Currenty / Currentx; - - //Find Throttle Point (the point where the throttle line and launch line intersect) - if (Currentx < 0) { - throttlePx = TDistance / sqrt(MLaunch * MLaunch + 1); - } else { - throttlePx = -(TDistance / sqrt(MLaunch * MLaunch + 1)); - } - - if (Currenty < 0) { - throttlePy = sqrt((TDistance * TDistance) - (throttlePx * throttlePx)); - } else { - throttlePy = -sqrt((TDistance * TDistance) - (throttlePx * throttlePx)); - } - - //Find ThrottleLine - ThrottleSlope = tan(atan2(Currenty, Currentx) + (3.14 / 2)); - ThrottleB = (throttlePy - (ThrottleSlope * throttlePx)); - - //Determine whether the UAV is below or above the throttle line - if (Currenty > ((ThrottleSlope * Currentx) + ThrottleB)) { - AboveLine = TRUE; - } else { - AboveLine = FALSE; - } - - //Translate the throttle point back - throttlePx = throttlePx + (WaypointX(BungeeWaypoint)); - throttlePy = throttlePy + (WaypointY(BungeeWaypoint)); - } - - //Find out if the UAV is currently above the line - if (Currenty > (ThrottleSlope * Currentx)) { - CurrentAboveLine = TRUE; - } else { - CurrentAboveLine = FALSE; - } - - //Find out if UAV has crossed the line - if (AboveLine != CurrentAboveLine && (*stateGetHorizontalSpeedNorm_f()) > Takeoff_MinSpeed) { + if (cross > 0. && (*stateGetHorizontalSpeedNorm_f()) > BUNGEE_TAKEOFF_MIN_SPEED) { CTakeoffStatus = Throttle; kill_throttle = 0; nav_init_stage(); + } else { + // If not crossed stay in this status + break; } - break; + // Start throttle imidiatelly case Throttle: //Follow Launch Line - NavVerticalAutoThrottleMode(AGR_CLIMB_PITCH); - NavVerticalThrottleMode(9600 * (1)); - nav_route_xy(initialx, initialy, throttlePx, throttlePy); + NavVerticalAutoThrottleMode(BUNGEE_TAKEOFF_PITCH); + NavVerticalThrottleMode(MAX_PPRZ * (BUNGEE_TAKEOFF_THROTTLE)); + nav_route_xy(init_point.x, init_point.y, throttle_point.x, throttle_point.y); kill_throttle = 0; - if ((stateGetPositionUtm_f()->alt > BungeeAlt + Takeoff_Height - 10) - && ((*stateGetHorizontalSpeedNorm_f()) > Takeoff_Speed)) { + if ((stateGetPositionUtm_f()->alt > bungee_point.z + BUNGEE_TAKEOFF_HEIGHT) +#if USE_AIRSPEED + && ((*stateGetAirspeed_f()) > BUNGEE_TAKEOFF_AIRSPEED) +#endif + ) { CTakeoffStatus = Finished; return FALSE; } else { @@ -221,7 +214,9 @@ bool_t nav_bungee_takeoff_run(void) } break; default: - break; + // Invalid status or Finished, end function + return FALSE; } return TRUE; } + diff --git a/sw/airborne/modules/nav/nav_bungee_takeoff.h b/sw/airborne/modules/nav/nav_bungee_takeoff.h index 37031c56f8..d7c9c50b6e 100644 --- a/sw/airborne/modules/nav/nav_bungee_takeoff.h +++ b/sw/airborne/modules/nav/nav_bungee_takeoff.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2008-2013 The Paparazzi Team + * Copyright (C) 2008-2015 The Paparazzi Team * * This file is part of paparazzi. * @@ -14,14 +14,45 @@ * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. + * along with paparazzi; see the file COPYING. If not, see + * . */ /** * @file modules/nav/nav_bungee_takeoff.h * + * Takeoff functions for bungee takeoff. + * + * Run initialize function when the plane is on the bungee, the bungee is + * fully extended and you are ready to launch the plane. + * After initialized, the plane will follow a line drawn by the position + * of the plane on initialization and the position of the bungee (given in + * the arguments). + * Once the plane crosses the throttle line, which is perpendicular to the line + * the plane is following, and intersects the position of the bungee (plus or + * minus a fixed distance (BUNGEE_TAKEOFF_DISTANCE in airframe file) from + * the bungee just in case the bungee doesn't release exactly above the bungee) + * the prop will come on. + * The plane will then continue to follow the line until it has reached a + * specific height (defined in as BUNGEE_TAKEOFF_HEIGHT in airframe file) above + * the bungee waypoint and airspeed (defined as BUNGEE_TAKEOFF_AIRSPEED in the + * airframe file). The airspeed limit is only used if USE_AIRSPEED flag is + * defined or set to true (and assuming the airspeed is then available). + * It is also possible to specify the pitch angle (BUNGEE_TAKEOFF_PITCH) and + * the throttle (BUNGEE_TAKEOFF_THROTTLE, between 0 and 1). + * + * @verbatim + *
+ * + * + * + * + * + * + *
+ * @endverbatim + * + * * from OSAM advanced navigation routines * */ @@ -31,7 +62,21 @@ #include "std.h" -extern bool_t nav_bungee_takeoff_setup(uint8_t BungeeWP); +/** Initialization function + * + * called in the flight plan before the 'run' function + * + * @param[in] bungee_wp Waypoint ID correcponding to the bungee location + * @return always false, since called only once by the flight plan + */ +extern bool_t nav_bungee_takeoff_setup(uint8_t bungee_wp); + +/** Bungee takeoff run function + * + * controls the different takeoff phases + * + * @return true until the takeoff procedure ends + */ extern bool_t nav_bungee_takeoff_run(void); #endif diff --git a/sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c b/sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c index 22a8ba7062..732e4d768c 100644 --- a/sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c +++ b/sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c @@ -202,10 +202,10 @@ void ms45xx_i2c_event(void) ms45xx.temperature = ((uint32_t)temp_raw * 2000) / 2047 - 500; // Send differential pressure via ABI - AbiSendMsgBARO_DIFF(MS45XX_SENDER_ID, &ms45xx.diff_pressure); + AbiSendMsgBARO_DIFF(MS45XX_SENDER_ID, ms45xx.diff_pressure); // Send temperature as float in deg Celcius via ABI float temp = ms45xx.temperature / 10.0f; - AbiSendMsgTEMPERATURE(MS45XX_SENDER_ID, &temp); + AbiSendMsgTEMPERATURE(MS45XX_SENDER_ID, temp); // Compute airspeed ms45xx.airspeed = sqrtf(Max(ms45xx.diff_pressure * ms45xx.airspeed_scale, 0)); diff --git a/sw/airborne/modules/sensors/baro_MS5534A.c b/sw/airborne/modules/sensors/baro_MS5534A.c index b6827a98ea..c6e1d4ef66 100644 --- a/sw/airborne/modules/sensors/baro_MS5534A.c +++ b/sw/airborne/modules/sensors/baro_MS5534A.c @@ -273,7 +273,7 @@ void baro_MS5534A_event(void) DOWNLINK_SEND_BARO_MS5534A(DefaultChannel, DefaultDevice, &baro_MS5534A_pressure, &baro_MS5534A_temp, &baro_MS5534A_z); #endif float pressure = (float)baro_MS5534A_pressure; - AbiSendMsgBARO_ABS(BARO_MS5534A_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_MS5534A_SENDER_ID, pressure); } } } diff --git a/sw/airborne/modules/sensors/baro_amsys.c b/sw/airborne/modules/sensors/baro_amsys.c index af28af453e..52d52bb760 100644 --- a/sw/airborne/modules/sensors/baro_amsys.c +++ b/sw/airborne/modules/sensors/baro_amsys.c @@ -169,7 +169,7 @@ void baro_amsys_read_event(void) baro_amsys_p = (float)(pBaroRaw - BARO_AMSYS_OFFSET_MIN) * BARO_AMSYS_MAX_PRESSURE / (float)( BARO_AMSYS_OFFSET_MAX - BARO_AMSYS_OFFSET_MIN); // Send pressure over ABI - AbiSendMsgBARO_ABS(BARO_AMSYS_SENDER_ID, &baro_amsys_p); + AbiSendMsgBARO_ABS(BARO_AMSYS_SENDER_ID, baro_amsys_p); // compute altitude localy if (!baro_amsys_offset_init) { --baro_amsys_cnt; diff --git a/sw/airborne/modules/sensors/baro_bmp.c b/sw/airborne/modules/sensors/baro_bmp.c index 22dee4d3e2..844c275099 100644 --- a/sw/airborne/modules/sensors/baro_bmp.c +++ b/sw/airborne/modules/sensors/baro_bmp.c @@ -90,9 +90,9 @@ void baro_bmp_event(void) baro_bmp_alt = 44330 * (1.0 - tmp); float pressure = (float)baro_bmp.pressure; - AbiSendMsgBARO_ABS(BARO_BMP_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_BMP_SENDER_ID, pressure); float temp = baro_bmp.temperature / 10.0f; - AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, &temp); + AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp); baro_bmp.data_available = FALSE; #ifdef SENSOR_SYNC_SEND diff --git a/sw/airborne/modules/sensors/baro_ets.c b/sw/airborne/modules/sensors/baro_ets.c index 24d70d5970..d672ac2799 100644 --- a/sw/airborne/modules/sensors/baro_ets.c +++ b/sw/airborne/modules/sensors/baro_ets.c @@ -187,7 +187,7 @@ void baro_ets_read_event(void) baro_ets_altitude = ground_alt + BARO_ETS_ALT_SCALE * (float)(baro_ets_offset - baro_ets_adc); // New value available float pressure = BARO_ETS_SCALE * (float) baro_ets_adc + BARO_ETS_PRESSURE_OFFSET; - AbiSendMsgBARO_ABS(BARO_ETS_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_ETS_SENDER_ID, pressure); #ifdef BARO_ETS_SYNC_SEND DOWNLINK_SEND_BARO_ETS(DefaultChannel, DefaultDevice, &baro_ets_adc, &baro_ets_offset, &baro_ets_altitude); #endif diff --git a/sw/airborne/modules/sensors/baro_hca.c b/sw/airborne/modules/sensors/baro_hca.c index fa514da67c..d802b257b0 100644 --- a/sw/airborne/modules/sensors/baro_hca.c +++ b/sw/airborne/modules/sensors/baro_hca.c @@ -98,7 +98,7 @@ void baro_hca_read_event(void) } float pressure = BARO_HCA_SCALE * (float)pBaroRaw + BARO_HCA_PRESSURE_OFFSET; - AbiSendMsgBARO_ABS(BARO_HCA_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_HCA_SENDER_ID, pressure); } baro_hca_i2c_trans.status = I2CTransDone; diff --git a/sw/airborne/modules/sensors/baro_mpl3115.c b/sw/airborne/modules/sensors/baro_mpl3115.c index d34a505747..fd2be6f12d 100644 --- a/sw/airborne/modules/sensors/baro_mpl3115.c +++ b/sw/airborne/modules/sensors/baro_mpl3115.c @@ -64,9 +64,9 @@ void baro_mpl3115_read_event(void) mpl3115_event(&baro_mpl); if (baro_mpl.data_available) { float pressure = (float)baro_mpl.pressure / (1 << 2); - AbiSendMsgBARO_ABS(BARO_MPL3115_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_MPL3115_SENDER_ID, pressure); float temp = (float)baro_mpl.pressure / 16.0f; - AbiSendMsgTEMPERATURE(BARO_MPL3115_SENDER_ID, &temp); + AbiSendMsgTEMPERATURE(BARO_MPL3115_SENDER_ID, temp); #ifdef SENSOR_SYNC_SEND DOWNLINK_SEND_MPL3115_BARO(DefaultChannel, DefaultDevice, &baro_mpl.pressure, &baro_mpl.temperature, &baro_mpl.alt); #endif diff --git a/sw/airborne/modules/sensors/baro_ms5611_i2c.c b/sw/airborne/modules/sensors/baro_ms5611_i2c.c index a9dfefe97a..8e4e9689ac 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_i2c.c +++ b/sw/airborne/modules/sensors/baro_ms5611_i2c.c @@ -94,9 +94,9 @@ void baro_ms5611_event(void) if (baro_ms5611.data_available) { float pressure = (float)baro_ms5611.data.pressure; - AbiSendMsgBARO_ABS(BARO_MS5611_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_MS5611_SENDER_ID, pressure); float temp = baro_ms5611.data.temperature / 100.0f; - AbiSendMsgTEMPERATURE(BARO_MS5611_SENDER_ID, &temp); + AbiSendMsgTEMPERATURE(BARO_MS5611_SENDER_ID, temp); baro_ms5611.data_available = FALSE; baro_ms5611_alt = pprz_isa_altitude_of_pressure(pressure); diff --git a/sw/airborne/modules/sensors/baro_ms5611_spi.c b/sw/airborne/modules/sensors/baro_ms5611_spi.c index 1d2bf01c6c..619a527bf0 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_spi.c +++ b/sw/airborne/modules/sensors/baro_ms5611_spi.c @@ -94,9 +94,9 @@ void baro_ms5611_event(void) if (baro_ms5611.data_available) { float pressure = (float)baro_ms5611.data.pressure; - AbiSendMsgBARO_ABS(BARO_MS5611_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_MS5611_SENDER_ID, pressure); float temp = baro_ms5611.data.temperature / 100.0f; - AbiSendMsgTEMPERATURE(BARO_MS5611_SENDER_ID, &temp); + AbiSendMsgTEMPERATURE(BARO_MS5611_SENDER_ID, temp); baro_ms5611.data_available = FALSE; baro_ms5611_alt = pprz_isa_altitude_of_pressure(pressure); diff --git a/sw/airborne/modules/sensors/baro_scp.c b/sw/airborne/modules/sensors/baro_scp.c index 97eccdcd4e..ef00a66a4f 100644 --- a/sw/airborne/modules/sensors/baro_scp.c +++ b/sw/airborne/modules/sensors/baro_scp.c @@ -189,7 +189,7 @@ void baro_scp_event(void) { if (baro_scp_available == TRUE) { float pressure = (float)baro_scp_pressure; - AbiSendMsgBARO_ABS(BARO_SCP_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_SCP_SENDER_ID, pressure); #ifdef SENSOR_SYNC_SEND DOWNLINK_SEND_SCP_STATUS(DefaultChannel, DefaultDevice, &baro_scp_pressure, &baro_scp_temperature); #endif diff --git a/sw/airborne/modules/sensors/baro_scp_i2c.c b/sw/airborne/modules/sensors/baro_scp_i2c.c index 58fdc78df7..49b5e3397c 100644 --- a/sw/airborne/modules/sensors/baro_scp_i2c.c +++ b/sw/airborne/modules/sensors/baro_scp_i2c.c @@ -102,7 +102,7 @@ void baro_scp_event(void) baro_scp_pressure *= 25; float pressure = (float) baro_scp_pressure; - AbiSendMsgBARO_ABS(BARO_SCP_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_SCP_SENDER_ID, pressure); #ifdef SENSOR_SYNC_SEND DOWNLINK_SEND_SCP_STATUS(DefaultChannel, DefaultDevice, &baro_scp_pressure, &baro_scp_temperature); #endif diff --git a/sw/airborne/modules/sensors/baro_sim.c b/sw/airborne/modules/sensors/baro_sim.c index 67ff539ab8..d8f214cc53 100644 --- a/sw/airborne/modules/sensors/baro_sim.c +++ b/sw/airborne/modules/sensors/baro_sim.c @@ -39,5 +39,5 @@ void baro_sim_init(void) void baro_sim_periodic(void) { float pressure = pprz_isa_pressure_of_altitude(gps.hmsl / 1000.0); - AbiSendMsgBARO_ABS(BARO_SIM_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_SIM_SENDER_ID, pressure); } diff --git a/sw/airborne/modules/sensors/pressure_board_navarro.c b/sw/airborne/modules/sensors/pressure_board_navarro.c index d4ec9cc5a6..ade7afb405 100644 --- a/sw/airborne/modules/sensors/pressure_board_navarro.c +++ b/sw/airborne/modules/sensors/pressure_board_navarro.c @@ -143,7 +143,7 @@ void pbn_read_event(void) } else { // Compute pressure float pressure = PBN_ALTITUDE_SCALE * (float) pbn.altitude_adc + PBN_PRESSURE_OFFSET; - AbiSendMsgBARO_ABS(BARO_PBN_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_PBN_SENDER_ID, pressure); // Compute airspeed and altitude //pbn_airspeed = (-4.45 + sqrtf(19.84-0.57*(float)(airspeed_offset-airspeed_adc)))/0.28; uint16_t diff = Max(pbn.airspeed_adc - pbn.airspeed_offset, 0); diff --git a/sw/airborne/modules/sonar/agl_dist.c b/sw/airborne/modules/sonar/agl_dist.c index 5825551388..724f864e5a 100644 --- a/sw/airborne/modules/sonar/agl_dist.c +++ b/sw/airborne/modules/sonar/agl_dist.c @@ -50,7 +50,7 @@ float agl_dist_value_filtered; abi_event sonar_ev; -static void sonar_cb(uint8_t sender_id, const float *distance); +static void sonar_cb(uint8_t sender_id, float distance); void agl_dist_init(void) { @@ -63,10 +63,10 @@ void agl_dist_init(void) } -static void sonar_cb(uint8_t __attribute__((unused)) sender_id, const float *distance) +static void sonar_cb(uint8_t __attribute__((unused)) sender_id, float distance) { - if (*distance < AGL_DIST_SONAR_MAX_RANGE && *distance > AGL_DIST_SONAR_MIN_RANGE) { - agl_dist_value = *distance; + if (distance < AGL_DIST_SONAR_MAX_RANGE && distance > AGL_DIST_SONAR_MIN_RANGE) { + agl_dist_value = distance; agl_dist_valid = TRUE; agl_dist_value_filtered = (AGL_DIST_SONAR_FILTER * agl_dist_value_filtered + agl_dist_value) / (AGL_DIST_SONAR_FILTER + 1); diff --git a/sw/airborne/modules/sonar/sonar_adc.c b/sw/airborne/modules/sonar/sonar_adc.c index 195fef52a6..028db175a1 100644 --- a/sw/airborne/modules/sonar/sonar_adc.c +++ b/sw/airborne/modules/sonar/sonar_adc.c @@ -76,7 +76,7 @@ void sonar_adc_read(void) #endif // SITL // Send ABI message - AbiSendMsgAGL(AGL_SONAR_ADC_ID, &sonar_adc.distance); + AbiSendMsgAGL(AGL_SONAR_ADC_ID, sonar_adc.distance); #ifdef SENSOR_SYNC_SEND_SONAR // Send Telemetry report diff --git a/sw/airborne/peripherals/ms5611_i2c.c b/sw/airborne/peripherals/ms5611_i2c.c index d328d58970..dbd1b2cce9 100644 --- a/sw/airborne/peripherals/ms5611_i2c.c +++ b/sw/airborne/peripherals/ms5611_i2c.c @@ -193,6 +193,8 @@ void ms5611_i2c_event(struct Ms5611_I2c *ms) ms->status = MS5611_STATUS_UNINIT; } } + } else { + ms->i2c_trans.status = I2CTransDone; } break; diff --git a/sw/airborne/subsystems/datalink/downlink.c b/sw/airborne/subsystems/datalink/downlink.c index b3a33cef16..f152fd2e47 100644 --- a/sw/airborne/subsystems/datalink/downlink.c +++ b/sw/airborne/subsystems/datalink/downlink.c @@ -65,7 +65,7 @@ void downlink_init(void) #if defined DATALINK #if DATALINK == PPRZ || DATALINK == SUPERBITRF || DATALINK == W5100 - pprz_transport_init(); + pprz_transport_init(&pprz_tp); #endif #if DATALINK == XBEE xbee_init(); @@ -75,6 +75,10 @@ void downlink_init(void) #endif #endif +#if USE_PPRZLOG + pprzlog_transport_init(); +#endif + #if SITL ivy_transport_init(); #endif diff --git a/sw/airborne/subsystems/datalink/pprz_transport.c b/sw/airborne/subsystems/datalink/pprz_transport.c index 75b19458ca..e995397c0b 100644 --- a/sw/airborne/subsystems/datalink/pprz_transport.c +++ b/sw/airborne/subsystems/datalink/pprz_transport.c @@ -114,18 +114,18 @@ static int check_available_space(struct pprz_transport *trans __attribute__((unu return dev->check_free_space(dev->periph, bytes); } -void pprz_transport_init(void) +void pprz_transport_init(struct pprz_transport *t) { - pprz_tp.status = UNINIT; - pprz_tp.trans_rx.msg_received = FALSE; - pprz_tp.trans_tx.size_of = (size_of_t) size_of; - pprz_tp.trans_tx.check_available_space = (check_available_space_t) check_available_space; - pprz_tp.trans_tx.put_bytes = (put_bytes_t) put_bytes; - pprz_tp.trans_tx.put_named_byte = (put_named_byte_t) put_named_byte; - pprz_tp.trans_tx.start_message = (start_message_t) start_message; - pprz_tp.trans_tx.end_message = (end_message_t) end_message; - pprz_tp.trans_tx.overrun = (overrun_t) overrun; - pprz_tp.trans_tx.count_bytes = (count_bytes_t) count_bytes; - pprz_tp.trans_tx.impl = (void *)(&pprz_tp); + t->status = UNINIT; + t->trans_rx.msg_received = FALSE; + t->trans_tx.size_of = (size_of_t) size_of; + t->trans_tx.check_available_space = (check_available_space_t) check_available_space; + t->trans_tx.put_bytes = (put_bytes_t) put_bytes; + t->trans_tx.put_named_byte = (put_named_byte_t) put_named_byte; + t->trans_tx.start_message = (start_message_t) start_message; + t->trans_tx.end_message = (end_message_t) end_message; + t->trans_tx.overrun = (overrun_t) overrun; + t->trans_tx.count_bytes = (count_bytes_t) count_bytes; + t->trans_tx.impl = (void *)(t); } diff --git a/sw/airborne/subsystems/datalink/pprz_transport.h b/sw/airborne/subsystems/datalink/pprz_transport.h index a505bd7413..04ecc41bd4 100644 --- a/sw/airborne/subsystems/datalink/pprz_transport.h +++ b/sw/airborne/subsystems/datalink/pprz_transport.h @@ -79,7 +79,7 @@ struct pprz_transport { extern struct pprz_transport pprz_tp; // Init function -extern void pprz_transport_init(void); +extern void pprz_transport_init(struct pprz_transport *t); static inline void parse_pprz(struct pprz_transport *t, uint8_t c) { diff --git a/sw/airborne/subsystems/datalink/pprzlog_transport.c b/sw/airborne/subsystems/datalink/pprzlog_transport.c index f6993bc4ad..1fa3b170b3 100644 --- a/sw/airborne/subsystems/datalink/pprzlog_transport.c +++ b/sw/airborne/subsystems/datalink/pprzlog_transport.c @@ -83,6 +83,7 @@ static void start_message(struct pprzlog_transport *trans, struct link_device *d const uint8_t msg_len = size_of(trans, payload_len); trans->ck = 0; put_1byte(trans, dev, msg_len); + put_1byte(trans, dev, 0); // TODO use correct source ID uint32_t ts = get_sys_time_usec() / 100; put_bytes(trans, dev, DL_TYPE_TIMESTAMP, DL_FORMAT_SCALAR, 4, (uint8_t *)(&ts)); } diff --git a/sw/airborne/subsystems/datalink/pprzlog_transport.h b/sw/airborne/subsystems/datalink/pprzlog_transport.h index ef96526f48..31e85c1328 100644 --- a/sw/airborne/subsystems/datalink/pprzlog_transport.h +++ b/sw/airborne/subsystems/datalink/pprzlog_transport.h @@ -41,5 +41,8 @@ struct pprzlog_transport { extern struct pprzlog_transport pprzlog_tp; +// Init function +extern void pprzlog_transport_init(void); + #endif diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c index 8166b5e0e8..848cfcb389 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.c +++ b/sw/airborne/subsystems/ins/ins_alt_float.c @@ -67,7 +67,7 @@ PRINT_CONFIG_MSG("USE_BAROMETER is TRUE: Using baro for altitude estimation.") #endif PRINT_CONFIG_VAR(INS_BARO_ID) abi_event baro_ev; -static void baro_cb(uint8_t sender_id, const float *pressure); +static void baro_cb(uint8_t sender_id, float pressure); #endif /* USE_BAROMETER */ static void alt_kalman_reset(void); @@ -138,7 +138,7 @@ void ins_reset_altitude_ref(void) #if USE_BAROMETER -static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) +static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure) { // timestamp in usec when last callback was received static uint32_t last_ts = 0; @@ -152,17 +152,17 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pres Bound(dt, 0.002, 1.0) if (!ins_impl.baro_initialized) { - ins_impl.qfe = *pressure; + ins_impl.qfe = pressure; ins_impl.baro_initialized = TRUE; } if (ins_impl.reset_alt_ref) { ins_impl.reset_alt_ref = FALSE; ins_impl.alt = ground_alt; ins_impl.alt_dot = 0.0f; - ins_impl.qfe = *pressure; + ins_impl.qfe = pressure; alt_kalman_reset(); } else { /* not realigning, so normal update with baro measurement */ - ins_impl.baro_alt = ground_alt + pprz_isa_height_of_pressure(*pressure, ins_impl.qfe); + ins_impl.baro_alt = ground_alt + pprz_isa_height_of_pressure(pressure, ins_impl.qfe); /* run the filter */ alt_kalman(ins_impl.baro_alt, dt); /* set new altitude, just copy old horizontal position */ diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.c b/sw/airborne/subsystems/ins/ins_float_invariant.c index 1c0007d96f..ccb64a5984 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant.c +++ b/sw/airborne/subsystems/ins/ins_float_invariant.c @@ -179,7 +179,7 @@ bool_t ins_baro_initialized; #endif PRINT_CONFIG_VAR(INS_BARO_ID) abi_event baro_ev; -static void baro_cb(uint8_t sender_id, const float *pressure); +static void baro_cb(uint8_t sender_id, float pressure); /* magnetometer */ #ifndef INS_MAG_ID @@ -515,7 +515,7 @@ void ins_update_gps(void) } -static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) +static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure) { static float ins_qfe = 101325.0f; static float alpha = 10.0f; @@ -527,10 +527,10 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pres // try to find a stable qfe // TODO generic function in pprz_isa ? if (i == 1) { - baro_moy = *pressure; - baro_prev = *pressure; + baro_moy = pressure; + baro_prev = pressure; } - baro_moy = (baro_moy * (i - 1) + *pressure) / i; + baro_moy = (baro_moy * (i - 1) + pressure) / i; alpha = (10.*alpha + (baro_moy - baro_prev)) / (11.0f); baro_prev = baro_moy; // test stop condition @@ -539,12 +539,12 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pres ins_baro_initialized = TRUE; } if (i == 250) { - ins_qfe = *pressure; + ins_qfe = pressure; ins_baro_initialized = TRUE; } i++; } else { /* normal update with baro measurement */ - ins_impl.meas.baro_alt = -pprz_isa_height_of_pressure(*pressure, ins_qfe); // Z down + ins_impl.meas.baro_alt = -pprz_isa_height_of_pressure(pressure, ins_qfe); // Z down } } diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index 35e2637c2b..ee73131fba 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -67,7 +67,7 @@ #define INS_SONAR_ID ABI_BROADCAST #endif abi_event sonar_ev; -static void sonar_cb(uint8_t sender_id, const float *distance); +static void sonar_cb(uint8_t sender_id, float distance); #ifdef INS_SONAR_THROTTLE_THRESHOLD #include "firmwares/rotorcraft/stabilization.h" @@ -114,7 +114,7 @@ PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE") #endif PRINT_CONFIG_VAR(INS_BARO_ID) abi_event baro_ev; -static void baro_cb(uint8_t sender_id, const float *pressure); +static void baro_cb(uint8_t sender_id, float pressure); struct InsInt ins_impl; @@ -271,22 +271,22 @@ void ins_propagate(float dt) ins_ned_to_state(); } -static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) +static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure) { - if (!ins_impl.baro_initialized && *pressure > 1e-7) { + if (!ins_impl.baro_initialized && pressure > 1e-7) { // wait for a first positive value - ins_impl.qfe = *pressure; + ins_impl.qfe = pressure; ins_impl.baro_initialized = TRUE; } if (ins_impl.baro_initialized) { if (ins_impl.vf_reset) { ins_impl.vf_reset = FALSE; - ins_impl.qfe = *pressure; + ins_impl.qfe = pressure; vff_realign(0.); ins_update_from_vff(); } else { - ins_impl.baro_z = -pprz_isa_height_of_pressure(*pressure, ins_impl.qfe); + ins_impl.baro_z = -pprz_isa_height_of_pressure(pressure, ins_impl.qfe); #if USE_VFF_EXTENDED vff_update_baro(ins_impl.baro_z); #else @@ -354,12 +354,12 @@ void ins_update_gps(void) #if USE_SONAR -static void sonar_cb(uint8_t __attribute__((unused)) sender_id, const float *distance) +static void sonar_cb(uint8_t __attribute__((unused)) sender_id, float distance) { static float last_offset = 0.; /* update filter assuming a flat ground */ - if (*distance < INS_SONAR_MAX_RANGE && *distance > INS_SONAR_MIN_RANGE + if (distance < INS_SONAR_MAX_RANGE && distance > INS_SONAR_MIN_RANGE #ifdef INS_SONAR_THROTTLE_THRESHOLD && stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD #endif @@ -368,7 +368,7 @@ static void sonar_cb(uint8_t __attribute__((unused)) sender_id, const float *dis #endif && ins_impl.update_on_agl && ins_impl.baro_initialized) { - vff_update_z_conf(-(*distance), VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabsf(*distance)); + vff_update_z_conf(-(distance), VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabsf(distance)); last_offset = vff.offset; } else { /* update offset with last value to avoid divergence */ diff --git a/sw/airborne/subsystems/navigation/waypoints.c b/sw/airborne/subsystems/navigation/waypoints.c new file mode 100644 index 0000000000..e2765bc91b --- /dev/null +++ b/sw/airborne/subsystems/navigation/waypoints.c @@ -0,0 +1,227 @@ +/* + * Copyright (C) 2015 Felix Ruess + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can 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, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file subsystems/navigation/waypoints.c + * + */ + +#include "subsystems/navigation/waypoints.h" +#include "state.h" +#include "subsystems/datalink/downlink.h" +#include "generated/flight_plan.h" + +const uint8_t nb_waypoint = NB_WAYPOINT; +struct Waypoint waypoints[NB_WAYPOINT]; + +/** initialize global and local waypoints */ +void waypoints_init(void) +{ + struct EnuCoor_f wp_tmp_float[NB_WAYPOINT] = WAYPOINTS_ENU; + struct LlaCoor_i wp_tmp_lla_i[NB_WAYPOINT] = WAYPOINTS_LLA_WGS84; + /* element in array is TRUE if absolute/global waypoint */ + bool_t is_global[NB_WAYPOINT] = WAYPOINTS_GLOBAL; + uint8_t i = 0; + for (i = 0; i < nb_waypoint; i++) { + /* clear all flags */ + waypoints[i].flags = 0; + /* init waypoint as global LLA or local ENU */ + if (is_global[i]) { + waypoint_set_global_flag(i); + nav_set_waypoint_lla(i, &wp_tmp_lla_i[i]); + } else { + nav_set_waypoint_enu_f(i, &wp_tmp_float[i]); + } + } +} + +void nav_set_waypoint_enu_i(uint8_t wp_id, struct EnuCoor_i *enu) +{ + if (wp_id < nb_waypoint) { + memcpy(&waypoints[wp_id].enu_i, enu, sizeof(struct EnuCoor_i)); + SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_I); + ENU_FLOAT_OF_BFP(waypoints[wp_id].enu_f, waypoints[wp_id].enu_i); + SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_F); + ClearBit(waypoints[wp_id].flags, WP_FLAG_LLA_I); + nav_globalize_local_wp(wp_id); + } +} + +void nav_set_waypoint_enu_f(uint8_t wp_id, struct EnuCoor_f *enu) +{ + if (wp_id < nb_waypoint) { + memcpy(&waypoints[wp_id].enu_f, enu, sizeof(struct EnuCoor_f)); + SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_I); + ENU_BFP_OF_REAL(waypoints[wp_id].enu_i, waypoints[wp_id].enu_f); + SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_F); + ClearBit(waypoints[wp_id].flags, WP_FLAG_LLA_I); + nav_globalize_local_wp(wp_id); + } +} + +void nav_move_waypoint_enu_i(uint8_t wp_id, struct EnuCoor_i *new_pos) +{ + if (wp_id < nb_waypoint) { + nav_set_waypoint_enu_i(wp_id, new_pos); + DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &(new_pos->x), + &(new_pos->y), &(new_pos->z)); + } +} + +/** + * Set only local XY coordinates of waypoint without update altitude. + * @TODO: how to handle global waypoints? + */ +void nav_set_waypoint_xy_i(uint8_t wp_id, int32_t x, int32_t y) +{ + if (wp_id < nb_waypoint) { + waypoints[wp_id].enu_i.x = x; + waypoints[wp_id].enu_i.y = y; + /* also update ENU float representation */ + waypoints[wp_id].enu_f.x = POS_FLOAT_OF_BFP(waypoints[wp_id].enu_i.x); + waypoints[wp_id].enu_f.y = POS_FLOAT_OF_BFP(waypoints[wp_id].enu_i.y); + nav_globalize_local_wp(wp_id); + } +} + +void nav_set_waypoint_lla(uint8_t wp_id, struct LlaCoor_i *lla) +{ + if (wp_id >= nb_waypoint) { + return; + } + memcpy(&waypoints[wp_id].lla, lla, sizeof(struct LlaCoor_i)); + SetBit(waypoints[wp_id].flags, WP_FLAG_LLA_I); + nav_localize_global_wp(wp_id); +} + +void nav_move_waypoint_lla(uint8_t wp_id, struct LlaCoor_i *lla) +{ + if (wp_id >= nb_waypoint) { + return; + } + nav_set_waypoint_lla(wp_id, lla); + if (nav_wp_is_global(wp_id)) { + /* lla->alt is above ellipsoid, WP_MOVED_LLA has hmsl alt */ + int32_t hmsl = lla->alt - state.ned_origin_i.lla.alt + state.ned_origin_i.hmsl; + DOWNLINK_SEND_WP_MOVED_LLA(DefaultChannel, DefaultDevice, &wp_id, + &lla->lat, &lla->lon, &hmsl); + } else { + DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, + &waypoints[wp_id].enu_i.x, + &waypoints[wp_id].enu_i.y, + &waypoints[wp_id].enu_i.z); + } +} + +/** set waypoint latitude/longitude without updating altitude */ +void nav_set_waypoint_latlon(uint8_t wp_id, struct LlaCoor_i *lla) +{ + if (wp_id >= nb_waypoint) { + return; + } + waypoints[wp_id].lla.lat = lla->lat; + waypoints[wp_id].lla.lon = lla->lon; + SetBit(waypoints[wp_id].flags, WP_FLAG_LLA_I); + nav_localize_global_wp(wp_id); +} + +/** set waypoint to current location and altitude */ +void nav_set_waypoint_here(uint8_t wp_id) +{ + if (wp_id >= nb_waypoint) { + return; + } + if (nav_wp_is_global(wp_id)) { + nav_set_waypoint_lla(wp_id, stateGetPositionLla_i()); + } else { + nav_set_waypoint_enu_i(wp_id, stateGetPositionEnu_i()); + } +} + +/** set waypoint to current horizontal location without modifying altitude */ +void nav_set_waypoint_here_2d(uint8_t wp_id) +{ + if (wp_id >= nb_waypoint) { + return; + } + if (nav_wp_is_global(wp_id)) { + nav_set_waypoint_latlon(wp_id, stateGetPositionLla_i()); + } else { + nav_set_waypoint_xy_i(wp_id, stateGetPositionEnu_i()->x, stateGetPositionEnu_i()->y); + } +} + +void nav_globalize_local_wp(uint8_t wp_id) +{ + if (state.ned_initialized_i) { + struct EcefCoor_i ecef; + ecef_of_enu_pos_i(&ecef, &state.ned_origin_i, &waypoints[wp_id].enu_i); + lla_of_ecef_i(&waypoints[wp_id].lla, &ecef); + SetBit(waypoints[wp_id].flags, WP_FLAG_LLA_I); + } +} + +/** update local ENU coordinates from its LLA coordinates */ +void nav_localize_global_wp(uint8_t wp_id) +{ + if (state.ned_initialized_i) { + struct EnuCoor_i enu; + enu_of_lla_point_i(&enu, &state.ned_origin_i, &waypoints[wp_id].lla); + // convert ENU pos from cm to BFP with INT32_POS_FRAC + enu.x = POS_BFP_OF_REAL(enu.x) / 100; + enu.y = POS_BFP_OF_REAL(enu.y) / 100; + enu.z = POS_BFP_OF_REAL(enu.z) / 100; + memcpy(&waypoints[wp_id].enu_i, &enu, sizeof(struct EnuCoor_i)); + SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_I); + ENU_FLOAT_OF_BFP(waypoints[wp_id].enu_f, waypoints[wp_id].enu_i); + SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_F); + } +} + +/** update local ENU coordinates of global waypoints */ +void nav_localize_global_waypoints(void) +{ + uint8_t i = 0; + for (i = 0; i < nb_waypoint; i++) { + if (nav_wp_is_global(i)) { + nav_localize_global_wp(i); + } + } +} + +/** Get LLA coordinates of waypoint. + * If the waypoint does not have its global coordinates set, + * the LLA representation is computed if the local origin is set. + * + * @param wp_id waypoint id + * @return pointer to waypoint LLA coordinates, NULL if invalid + */ +struct LlaCoor_i *nav_get_waypoint_lla(uint8_t wp_id) +{ + if (wp_id < nb_waypoint) { + if (!bit_is_set(waypoints[wp_id].flags, WP_FLAG_LLA_I)) { + nav_globalize_local_wp(wp_id); + } + return &waypoints[wp_id].lla; + } + else { + return NULL; + } +} diff --git a/sw/airborne/subsystems/navigation/waypoints.h b/sw/airborne/subsystems/navigation/waypoints.h new file mode 100644 index 0000000000..ce06a67848 --- /dev/null +++ b/sw/airborne/subsystems/navigation/waypoints.h @@ -0,0 +1,110 @@ +/* + * Copyright (C) 2015 Felix Ruess + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can 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, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, see + * . + */ + +/** + * @file subsystems/navigation/waypoints.h + * + */ + +#ifndef WAYPOINTS_H +#define WAYPOINTS_H + +#include "std.h" +#include "math/pprz_geodetic_int.h" +#include "math/pprz_geodetic_float.h" + +#define WP_FLAG_GLOBAL 0 +#define WP_FLAG_ENU_I 1 +#define WP_FLAG_ENU_F 2 +#define WP_FLAG_LLA_I 3 + +struct Waypoint { + uint8_t flags; ///< bitmask encoding valid representations and if local or global + struct EnuCoor_i enu_i; ///< with #INT32_POS_FRAC + struct EnuCoor_f enu_f; + struct LlaCoor_i lla; +}; + +extern const uint8_t nb_waypoint; +/** size == nb_waypoint, waypoint 0 is a dummy waypoint */ +extern struct Waypoint waypoints[]; + +#define WaypointX(_wp) waypoints[_wp].enu_f.x +#define WaypointY(_wp) waypoints[_wp].enu_f.y +#define WaypointAlt(_wp) waypoints[_wp].enu_f.z +#define Height(_h) (_h) + +static inline bool_t nav_wp_is_global(uint8_t wp_id) +{ + if (wp_id < nb_waypoint) { + return bit_is_set(waypoints[wp_id].flags, WP_FLAG_GLOBAL); + } + return FALSE; +} + +static inline void waypoint_set_global_flag(uint8_t wp_id) +{ + if (wp_id < nb_waypoint) { + SetBit(waypoints[wp_id].flags, WP_FLAG_GLOBAL); + } +} + +static inline void waypoint_clear_global_flag(uint8_t wp_id) +{ + if (wp_id < nb_waypoint) { + ClearBit(waypoints[wp_id].flags, WP_FLAG_GLOBAL); + } +} + +extern void waypoints_init(void); + +extern void nav_set_waypoint_enu_f(uint8_t wp_id, struct EnuCoor_f *enu); +extern void nav_set_waypoint_enu_i(uint8_t wp_id, struct EnuCoor_i *enu); +extern void nav_set_waypoint_xy_i(uint8_t wp_id, int32_t x, int32_t y); +extern void nav_move_waypoint_enu_i(uint8_t wp_id, struct EnuCoor_i *new_pos); +extern void nav_set_waypoint_lla(uint8_t wp_id, struct LlaCoor_i *lla); +extern void nav_move_waypoint_lla(uint8_t wp_id, struct LlaCoor_i *lla); +/** set waypoint latitude/longitude without updating altitude */ +void nav_set_waypoint_latlon(uint8_t wp_id, struct LlaCoor_i *lla); + +/** set waypoint to current location and altitude */ +extern void nav_set_waypoint_here(uint8_t wp_id); + +/** set waypoint to current horizontal location without modifying altitude */ +extern void nav_set_waypoint_here_2d(uint8_t wp_id); + +/** update global LLA coordinates from its ENU coordinates */ +extern void nav_globalize_local_wp(uint8_t wp_id); + +/** update local ENU coordinates from its LLA coordinates */ +extern void nav_localize_global_wp(uint8_t wp_id); +/** update local ENU coordinates of global waypoints */ +extern void nav_localize_global_waypoints(void); + +/** Get LLA coordinates of waypoint. + * If the waypoint does not have its global coordinates set, + * the LLA representation is computed if the local origin is set. + * + * @param wp_id waypoint id + * @return pointer to waypoint LLA coordinates, NULL if invalid + */ +extern struct LlaCoor_i *nav_get_waypoint_lla(uint8_t wp_id); + +#endif /* WAYPOINTS_H */ diff --git a/sw/airborne/test/test_baro_board.c b/sw/airborne/test/test_baro_board.c index abed668fb2..f1f24ee26b 100644 --- a/sw/airborne/test/test_baro_board.c +++ b/sw/airborne/test/test_baro_board.c @@ -78,9 +78,9 @@ int main(void) return 0; } -static void pressure_abs_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) +static void pressure_abs_cb(uint8_t __attribute__((unused)) sender_id, float pressure) { - float p = *pressure; + float p = pressure; float foo = 42.; DOWNLINK_SEND_BARO_RAW(DefaultChannel, DefaultDevice, &p, &foo); } diff --git a/sw/ext/libopencm3 b/sw/ext/libopencm3 index 40d9ffcb6b..4b8f6e01ab 160000 --- a/sw/ext/libopencm3 +++ b/sw/ext/libopencm3 @@ -1 +1 @@ -Subproject commit 40d9ffcb6bcae58f9dcbe1d5bce70f04e05f2977 +Subproject commit 4b8f6e01abeefbf226a8a10a9039a15363bc888e diff --git a/sw/ext/luftboot b/sw/ext/luftboot index f7a961e3ea..6af6ab63a3 160000 --- a/sw/ext/luftboot +++ b/sw/ext/luftboot @@ -1 +1 @@ -Subproject commit f7a961e3ea6c04f67320e65515a82473960c500b +Subproject commit 6af6ab63a36557a3cc894777129351fb8b5f89d2 diff --git a/sw/ground_segment/cockpit/gcs.ml b/sw/ground_segment/cockpit/gcs.ml index 0ca6468b29..152af3917d 100644 --- a/sw/ground_segment/cockpit/gcs.ml +++ b/sw/ground_segment/cockpit/gcs.ml @@ -338,6 +338,7 @@ and display_particules = ref false and wid = ref None and srtm = ref false and hide_fp = ref false +and timestamp = ref false let options = [ @@ -371,6 +372,7 @@ let options = "-wid", Arg.String (fun s -> wid := Some (Int32.of_string s)), " Id of an existing window to be attached to"; "-zoom", Arg.Set_float zoom, "Initial zoom"; "-auto_hide_fp", Arg.Unit (fun () -> Live.auto_hide_fp true; hide_fp := true), "Automatically hide flight plans of unselected aircraft"; + "-timestamp", Arg.Set timestamp, "Bind on timestampped telemetry messages"; ] @@ -571,6 +573,13 @@ let rec update_widget_size = fun orientation widgets xml -> | x -> failwith (sprintf "update_widget_size: %s" x) +(* get DTD head line for layout *) +let get_layout_dtd = fun filename -> + let gcs_regexp = Str.regexp (Filename.concat Env.paparazzi_home "conf/gcs") in + let local_dir = Str.replace_first gcs_regexp "" (Filename.dirname filename) in + let split = Str.split (Str.regexp Filename.dir_sep) local_dir in + let layout = List.fold_left (fun s _ -> "../" ^ s ) "layout.dtd" split in + sprintf "" layout let save_layout = fun filename contents -> @@ -585,6 +594,7 @@ let save_layout = fun filename contents -> `SAVE, Some name -> dialog#destroy (); let f = open_out name in + fprintf f "%s\n\n" (get_layout_dtd name); fprintf f "%s\n" contents; close_out f | _ -> dialog#destroy () @@ -693,7 +703,15 @@ let () = listen_dropped_papgets geomap; let save_layout = fun () -> - let the_new_layout = replace_widget_children "map2d" (Papgets.dump_store ()) the_layout in + (* Ask if ac_id parameters from papgets should be saved *) + let save_acid = + if Papgets.has_papgets () then + match GToolbox.question_box ~title:"Save Layout" ~buttons:["Yes"; "no"] ~default:1 "Do you want to save A/C id of Papgets if available\nYes: the saved layout will only work with A/C that have the same id (default)\nno: the saved layout will work with any A/C (but will mix data while using multiple A/C)" with + | 2 -> false + | _ -> true + else true + in + let the_new_layout = replace_widget_children "map2d" (Papgets.dump_store save_acid) the_layout in let width, height = Gdk.Drawable.get_size window#misc#window in let the_new_layout = update_widget_size `HORIZONTAL widgets the_new_layout in let new_layout = Xml.Element ("layout", ["width", soi width; "height", soi height], [the_new_layout]) in @@ -755,7 +773,7 @@ let () = begin my_alert#add "Waiting for telemetry..."; Speech.say "Waiting for telemetry..."; - Live.listen_acs_and_msgs geomap ac_notebook my_alert !auto_center_new_ac alt_graph + Live.listen_acs_and_msgs geomap ac_notebook my_alert !auto_center_new_ac alt_graph !timestamp end; (** Display the window *) diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml index 244db7a4d5..4a77d9b001 100644 --- a/sw/ground_segment/cockpit/live.ml +++ b/sw/ground_segment/cockpit/live.ml @@ -545,6 +545,7 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id (* Drag for Drop *) let papget = Papget_common.xml "goto_block" "button" [ "block_name", block_name; + "ac_id", ac_id; "icon", icon] in Papget_common.dnd_source b#coerce papget; @@ -621,7 +622,7 @@ let create_ac = fun alert (geomap:G.widget) (acs_notebook:GPack.notebook) (ac_id let dl_settings_page = try let xml_settings = Xml.children (ExtXml.child settings_xml "dl_settings") in - let settings_tab = new Page_settings.settings ~visible xml_settings dl_setting_callback (fun group x -> strip#add_widget ~group x) in + let settings_tab = new Page_settings.settings ~visible xml_settings dl_setting_callback ac_id (fun group x -> strip#add_widget ~group x) in (** Connect key shortcuts *) let key_press = fun ev -> @@ -789,12 +790,12 @@ let alert_bind = fun msg cb -> try cb sender vs with _ -> () in ignore (Alert_Pprz.message_bind msg safe_cb) -let tele_bind = fun msg cb -> +let tele_bind = fun msg cb timestamp -> let safe_cb = fun sender vs -> try cb sender vs with AC_not_found -> () (* A/C not yet registed; silently ignore *) | x -> fprintf stderr "tele_bind (%s): %s\n%!" msg (Printexc.to_string x) in - ignore (Tele_Pprz.message_bind msg safe_cb) + ignore (Tele_Pprz.message_bind ~timestamp msg safe_cb) let ask_config = fun alert geomap fp_notebook ac -> let get_config = fun _sender values -> @@ -1265,7 +1266,7 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph -> let color = match ap_mode with "AUTO2" | "NAV" -> ok_color - | "AUTO1" | "R_RCC" | "A_RCC" | "ATT_C" | "R_ZH" | "A_ZH" | "HOVER" | "HOV_C" | "H_ZH" -> "#10F0E0" + | "AUTO1" | "R_RCC" | "A_RCC" | "ATT_C" | "R_ZH" | "A_ZH" | "HOVER" | "HOV_C" | "H_ZH" | "MODULE" -> "#10F0E0" | "MANUAL" | "RATE" | "ATT" | "RC_D" | "CF" | "FWD" -> warning_color | _ -> alert_color in ac.strip#set_color "AP" color; @@ -1374,8 +1375,8 @@ let mark_dcshot = fun (geomap:G.widget) _sender vs -> (* mark geomap ac.ac_name track !Plugin.frame *) -let listen_dcshot = fun _geom -> - tele_bind "DC_SHOT" (mark_dcshot _geom) +let listen_dcshot = fun _geom timestamp -> + tele_bind "DC_SHOT" (mark_dcshot _geom) timestamp let listen_error = fun a -> let get_error = fun _sender vs -> @@ -1383,14 +1384,14 @@ let listen_error = fun a -> log_and_say a "gcs" msg in safe_bind "TELEMETRY_ERROR" get_error -let listen_info_msg = fun a -> +let listen_info_msg = fun a timestamp -> let get_msg = fun a _sender vs -> let ac = find_ac _sender in let msg_array = Pprz.assoc "msg" vs in log_and_say a ac.ac_name (Pprz.string_of_value msg_array) in - tele_bind "INFO_MSG" (get_msg a) + tele_bind "INFO_MSG" (get_msg a) timestamp -let listen_autopilot_version_msg = fun a -> +let listen_autopilot_version_msg = fun a timestamp -> let get_msg = fun a _sender vs -> let ac = find_ac _sender in let desc_array = Pprz.assoc "desc" vs in @@ -1398,9 +1399,9 @@ let listen_autopilot_version_msg = fun a -> if ac.version <> version then log a ac.ac_name (sprintf "%s version:\n%s" ac.ac_name version); ac.version <- version in - tele_bind "AUTOPILOT_VERSION" (get_msg a) + tele_bind "AUTOPILOT_VERSION" (get_msg a) timestamp -let listen_tcas = fun a -> +let listen_tcas = fun a timestamp -> let get_alarm_tcas = fun a txt _sender vs -> let ac = find_ac _sender in let other_ac = get_ac vs in @@ -1413,10 +1414,10 @@ let listen_tcas = fun a -> with _ -> "" in log_and_say a ac.ac_name (sprintf "%s : %s -> %s %s" txt ac.ac_speech_name other_ac.ac_speech_name resolve) in - tele_bind "TCAS_TA" (get_alarm_tcas a "tcas TA"); - tele_bind "TCAS_RA" (get_alarm_tcas a "TCAS RA") + tele_bind "TCAS_TA" (get_alarm_tcas a "tcas TA") timestamp; + tele_bind "TCAS_RA" (get_alarm_tcas a "TCAS RA") timestamp -let listen_acs_and_msgs = fun geomap ac_notebook my_alert auto_center_new_ac alt_graph -> +let listen_acs_and_msgs = fun geomap ac_notebook my_alert auto_center_new_ac alt_graph timestamp -> (** Probe live A/Cs *) let probe = fun () -> message_request "gcs" "AIRCRAFTS" [] (fun _sender vs -> aircrafts_msg my_alert geomap ac_notebook vs) in @@ -1436,10 +1437,10 @@ let listen_acs_and_msgs = fun geomap ac_notebook my_alert auto_center_new_ac alt listen_svsinfo my_alert; listen_alert my_alert; listen_error my_alert; - listen_info_msg my_alert; - listen_autopilot_version_msg my_alert; - listen_tcas my_alert; - listen_dcshot geomap; + listen_info_msg my_alert timestamp; + listen_autopilot_version_msg my_alert timestamp; + listen_tcas my_alert timestamp; + listen_dcshot geomap timestamp; (** Select the active aircraft on notebook page selection *) let callback = fun i -> diff --git a/sw/ground_segment/cockpit/live.mli b/sw/ground_segment/cockpit/live.mli index b00bf2b580..4b2df8d279 100644 --- a/sw/ground_segment/cockpit/live.mli +++ b/sw/ground_segment/cockpit/live.mli @@ -75,8 +75,8 @@ val track_size : int ref val auto_hide_fp : bool -> unit (** Automatically hide flight plan of not selected ac *) -val listen_acs_and_msgs : MapCanvas.widget -> GPack.notebook -> Pages.alert -> bool -> Gtk_tools.pixmap_in_drawin_area -> unit -(** [listen_acs_and_msgs geomap aircraft_notebook alert_page auto_center_new_ac alt_graph] *) +val listen_acs_and_msgs : MapCanvas.widget -> GPack.notebook -> Pages.alert -> bool -> Gtk_tools.pixmap_in_drawin_area -> bool -> unit +(** [listen_acs_and_msgs geomap aircraft_notebook alert_page auto_center_new_ac alt_graph timestamp] *) val jump_to_block : string -> int -> unit (** [jump_to_block ac_id block_id] Sends a JUMP_TO_BLOCK message *) diff --git a/sw/ground_segment/cockpit/page_settings.ml b/sw/ground_segment/cockpit/page_settings.ml index 5d71f3d373..4b7a15a1dc 100644 --- a/sw/ground_segment/cockpit/page_settings.ml +++ b/sw/ground_segment/cockpit/page_settings.ml @@ -80,7 +80,7 @@ let add_key = fun xml do_change keys -> -let one_setting = fun (i:int) (do_change:int -> float -> unit) packing dl_setting (tooltips:GData.tooltips) strip keys -> +let one_setting = fun (i:int) (do_change:int -> float -> unit) ac_id packing dl_setting (tooltips:GData.tooltips) strip keys -> let f = fun a -> float_of_string (ExtXml.attrib dl_setting a) in let lower = f "min" and upper = f "max" @@ -257,6 +257,7 @@ let one_setting = fun (i:int) (do_change:int -> float -> unit) packing dl_settin let papget = Papget_common.xml "variable_setting" "button" ["variable", varname; "value", ExtXml.attrib x "value"; + "ac_id", ac_id; "icon", icon] in Papget_common.dnd_source b#coerce papget; @@ -287,12 +288,12 @@ let same_tag_for_all = function (** Build the tree of settings *) -let rec build_settings = fun do_change i flat_list keys xml_settings packing tooltips strip -> +let rec build_settings = fun do_change ac_id i flat_list keys xml_settings packing tooltips strip -> match same_tag_for_all xml_settings with "dl_setting" -> List.iter (fun dl_setting -> - let label_value = one_setting !i do_change packing dl_setting tooltips strip keys in + let label_value = one_setting !i do_change ac_id packing dl_setting tooltips strip keys in flat_list := label_value :: !flat_list; incr i) xml_settings @@ -308,18 +309,18 @@ let rec build_settings = fun do_change i flat_list keys xml_settings packing too ignore (n#append_page ~tab_label vbox#coerce); let children = Xml.children dl_settings in - build_settings do_change i flat_list keys children vbox#pack tooltips strip) + build_settings do_change ac_id i flat_list keys children vbox#pack tooltips strip) xml_settings | tag -> failwith (sprintf "Page_settings.build_settings, unexpected tag '%s'" tag) -class settings = fun ?(visible = fun _ -> true) xml_settings do_change strip -> +class settings = fun ?(visible = fun _ -> true) xml_settings do_change ac_id strip -> let sw = GBin.scrolled_window ~hpolicy:`AUTOMATIC ~vpolicy:`AUTOMATIC () in let vbox = GPack.vbox ~packing:sw#add_with_viewport () in let tooltips = GData.tooltips () in let i = ref 0 and l = ref [] and keys = ref [] in let ordered_list = - build_settings do_change i l keys xml_settings vbox#add tooltips strip; + build_settings do_change ac_id i l keys xml_settings vbox#add tooltips strip; List.rev !l in let variables = Array.of_list ordered_list in let length = Array.length variables in diff --git a/sw/ground_segment/cockpit/page_settings.mli b/sw/ground_segment/cockpit/page_settings.mli index 73b1600ba5..6f80de8568 100644 --- a/sw/ground_segment/cockpit/page_settings.mli +++ b/sw/ground_segment/cockpit/page_settings.mli @@ -23,7 +23,7 @@ *) (** [new Page_settings.settings ?visible dl_settings callback short_button_receiver] *) -class settings : ?visible:(GObj.widget -> bool) -> Xml.xml list -> (int -> float -> unit) -> (string -> GObj.widget -> unit) -> +class settings : ?visible:(GObj.widget -> bool) -> Xml.xml list -> (int -> float -> unit) -> string -> (string -> GObj.widget -> unit) -> object method length : int (** Total number of settings *) method set : int -> string option -> unit (** Set the current value *) diff --git a/sw/ground_segment/cockpit/papgets.ml b/sw/ground_segment/cockpit/papgets.ml index 120fdc2b52..7ab9945deb 100644 --- a/sw/ground_segment/cockpit/papgets.ml +++ b/sw/ground_segment/cockpit/papgets.ml @@ -25,26 +25,37 @@ open Printf module PC = Papget_common +let filter_acid = fun save conf -> + let filtered = List.filter (fun x -> + (* keep element if save is true or save is false and attrib name is not ac_id *) + if (ExtXml.attrib_or_default x "name" "" = "ac_id") && (not save) then false + else true) (Xml.children conf) in + Xml.Element (Xml.tag conf, Xml.attribs conf, filtered) + let papgets = Hashtbl.create 5 let register_papget = fun p -> Hashtbl.add papgets p p -let dump_store = fun () -> +let dump_store = fun save_id -> Hashtbl.fold (fun _ p r -> if not p#deleted then - p#config ()::r + (filter_acid save_id (p#config ()))::r else r) papgets [] +let has_papgets = fun () -> + (Hashtbl.fold (fun _ p n -> if p#deleted then n else n + 1) papgets 0) > 0 + let papget_listener = let sep = Str.regexp "[:\\.]" in fun papget -> try let field = Papget_common.get_property "field" papget in + let sender = try Some (Papget_common.get_property "ac_id" papget) with _ -> None in match Str.split sep field with [msg_name; field_name] -> - (new Papget.message_field msg_name field_name) + (new Papget.message_field ?sender msg_name field_name) | _ -> failwith (sprintf "Unexpected field spec: %s" field) with _ -> failwith (sprintf "field attr expected in '%s" (Xml.to_string papget)) @@ -72,7 +83,8 @@ let extra_functions = let expression_listener = fun papget -> let expr = Papget_common.get_property "expr" papget in let expr = Expr_lexer.parse expr in - new Papget.expression ~extra_functions expr + let sender = try Some (Papget_common.get_property "ac_id" papget) with _ -> None in + new Papget.expression ~extra_functions ?sender expr @@ -100,6 +112,11 @@ let locked = fun config -> [PC.property "locked" (PC.get_property "locked" config)] with _ -> [] +let ac_id_prop = fun config -> + try + [PC.property "ac_id" (PC.get_property "ac_id" config)] + with _ -> [] + let create = fun canvas_group papget -> try let type_ = ExtXml.attrib papget "type" @@ -124,18 +141,21 @@ let create = fun canvas_group papget -> | _ -> failwith (sprintf "Unexpected papget display: %s" display) in let block_name = Papget_common.get_property "block_name" papget in let clicked = fun () -> - prerr_endline "Warning: goto_block papget sends to all A/C"; - Hashtbl.iter - (fun ac_id ac -> - let blocks = ExtXml.child ac.Live.fp "blocks" in - let block = ExtXml.child ~select:(fun x -> ExtXml.attrib x "name" = block_name) blocks "block" in - let block_id = ExtXml.int_attrib block "no" in - Live.jump_to_block ac_id block_id - ) - Live.aircrafts + let jump_to_block = fun ac_id ac -> + let blocks = ExtXml.child ac.Live.fp "blocks" in + let block = ExtXml.child ~select:(fun x -> ExtXml.attrib x "name" = block_name) blocks "block" in + let block_id = ExtXml.int_attrib block "no" in + Live.jump_to_block ac_id block_id + in + let sender = try Some (Papget_common.get_property "ac_id" papget) with _ -> None in + match sender with + Some ac_id -> begin try jump_to_block ac_id (Hashtbl.find Live.aircrafts ac_id) with _ -> () end + | None -> + prerr_endline "Warning: goto_block papget sends to all active A/C"; + Hashtbl.iter jump_to_block Live.aircrafts in let properties = - [ Papget_common.property "block_name" block_name ] @ locked papget in + [ Papget_common.property "block_name" block_name ] @ locked papget @ ac_id_prop papget in let p = new Papget.canvas_goto_block_item properties clicked renderer in let p = (p :> Papget.item) in @@ -151,20 +171,24 @@ let create = fun canvas_group papget -> and value = float_of_string (Papget_common.get_property "value" papget) in let clicked = fun () -> - prerr_endline "Warning: variable_setting papget sending to all active A/C"; - Hashtbl.iter - (fun ac_id ac -> - match ac.Live.dl_settings_page with - None -> () - | Some settings -> - let var_id = settings#assoc varname in - Live.dl_setting ac_id var_id value) - Live.aircrafts + let send_setting = fun ac_id ac -> + match ac.Live.dl_settings_page with + None -> () + | Some settings -> + let var_id = settings#assoc varname in + Live.dl_setting ac_id var_id value + in + let sender = try Some (Papget_common.get_property "ac_id" papget) with _ -> None in + match sender with + Some ac_id -> begin try send_setting ac_id (Hashtbl.find Live.aircrafts ac_id) with _ -> () end + | None -> + prerr_endline "Warning: variable_setting papget sending to all active A/C"; + Hashtbl.iter send_setting Live.aircrafts in let properties = [ Papget_common.property "variable" varname; Papget_common.float_property "value" value ] - @ locked papget in + @ locked papget @ ac_id_prop papget in let p = new Papget.canvas_variable_setting_item properties clicked renderer in let p = (p :> Papget.item) in register_papget p @@ -198,13 +222,14 @@ let parse_message_dnd = | _ -> raise (Parse_message_dnd (Printf.sprintf "parse_dnd: %s" s)) let dnd_data_received = fun canvas_group _context ~x ~y data ~info ~time -> try (* With the format sent by Messages *) - let (_sender, _class_name, msg_name, field_name,scale) = parse_message_dnd data#data in + let (sender, _class_name, msg_name, field_name,scale) = parse_message_dnd data#data in let attrs = [ "type", "message_field"; "display", "text"; "x", sprintf "%d" x; "y", sprintf "%d" y ] and props = [ Papget_common.property "field" (sprintf "%s:%s" msg_name field_name); + Papget_common.property "ac_id" sender; Papget_common.property "scale" scale ] in let papget_xml = Xml.Element ("papget", attrs, props) in create canvas_group papget_xml diff --git a/sw/ground_segment/cockpit/papgets.mli b/sw/ground_segment/cockpit/papgets.mli index 5afbaa143e..9f64c18590 100644 --- a/sw/ground_segment/cockpit/papgets.mli +++ b/sw/ground_segment/cockpit/papgets.mli @@ -22,7 +22,8 @@ * *) -val dump_store : unit -> Xml.xml list +val dump_store : bool -> Xml.xml list +val has_papgets : unit -> bool val create : #GnoCanvas.group -> Xml.xml -> unit val dnd_data_received : #GnoCanvas.group -> diff --git a/sw/ground_segment/python/ivytoredis/ivy_to_redis.py b/sw/ground_segment/python/ivytoredis/ivy_to_redis.py new file mode 100755 index 0000000000..285bc508c4 --- /dev/null +++ b/sw/ground_segment/python/ivytoredis/ivy_to_redis.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python + +from __future__ import print_function + +import redis +import time +import signal +import argparse +import sys +import os + +# if PAPARAZZI_SRC not set, then assume the tree containing this +# file is a reasonable substitute +PPRZ_SRC = os.getenv("PAPARAZZI_SRC", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), + '../../../../'))) +PPRZ_LIB_PYTHON = os.path.join(PPRZ_SRC, "sw/lib/python") +sys.path.append(PPRZ_LIB_PYTHON) + +from ivy_msg_interface import IvyMessagesInterface + +server = None + + +class Ivy2RedisServer(): + def __init__(self, redishost, redisport, verbose=False): + self.verbose = verbose + self.interface = IvyMessagesInterface(self.message_recv) + self.r = redis.StrictRedis(host=redishost, port=redisport, db=0) + self.keep_running = True + print("Connected to redis server %s on port %i" % (redishost, redisport)) + + def message_recv(self, ac_id, msg): + # if ac_id is not 0 (i.e. telemetry from an aircraft) include it in the key + # don't add it to the key for ground messages + if ac_id: + key = "{0}.{1}.{2}".format(msg.get_classname(), msg.get_msgname(), ac_id) + else: + key = "{0}.{1}".format(msg.get_classname(), msg.get_msgname()) + if self.verbose: + print("received message, key=%s, msg=%s" % (key, msg.to_json(payload_only=True))) + sys.stdout.flush() + self.r.publish(key, msg.to_json(payload_only=True)) + self.r.set(key, msg.to_json(payload_only=True)) + + def run(self): + while self.keep_running: + time.sleep(0.1) + + def stop(self): + self.keep_running = False + self.interface.shutdown() + + +def signal_handler(signal, frame): + global server + server.stop() + + +def main(): + global server + parser = argparse.ArgumentParser() + parser.add_argument("-s", "--server", help="hostname here redis runs", default="localhost") + parser.add_argument("-p", "--port", help="port used by redis", type=int, default=6379) + parser.add_argument("-v", "--verbose", dest="verbose", action="store_true") + args = parser.parse_args() + server = Ivy2RedisServer(args.server, args.port, args.verbose) + signal.signal(signal.SIGINT, signal_handler) + server.run() + +if __name__ == '__main__': + main() diff --git a/sw/ground_segment/python/messages_app/messagesapp.py b/sw/ground_segment/python/messages_app/messagesapp.py index 6b44cb483a..3c26c27961 100755 --- a/sw/ground_segment/python/messages_app/messagesapp.py +++ b/sw/ground_segment/python/messages_app/messagesapp.py @@ -1,23 +1,17 @@ #!/usr/bin/env python import wx -import getopt -import sys import messagesframe + class MessagesApp(wx.App): def OnInit(self): self.main = messagesframe.MessagesFrame() self.main.Show() self.SetTopWindow(self.main) - #opts, args = getopt.getopt(sys.argv[1:], "p:", - #["plot"]) - #for o,a in opts: - #if o in ("-p", "--plot"): - #[ac_id, message, field, color, use_x] = a.split(':') - #self.main.AddPlot(int(ac_id), message, field, color, bool(int(use_x))) return True + def main(): application = MessagesApp(0) application.MainLoop() diff --git a/sw/ground_segment/python/messages_app/messagesframe.py b/sw/ground_segment/python/messages_app/messagesframe.py index ac1e32573e..4a1862ea69 100644 --- a/sw/ground_segment/python/messages_app/messagesframe.py +++ b/sw/ground_segment/python/messages_app/messagesframe.py @@ -1,15 +1,20 @@ import wx import sys -import os import time import threading -import math -PPRZ_HOME = os.getenv("PAPARAZZI_HOME") -sys.path.append(PPRZ_HOME + "/sw/lib/python") +from os import path, getenv -import messages_tool +# if PAPARAZZI_SRC not set, then assume the tree containing this +# file is a reasonable substitute +PPRZ_SRC = getenv("PAPARAZZI_SRC", path.normpath(path.join(path.dirname(path.abspath(__file__)), '../../../../'))) +sys.path.append(PPRZ_SRC + "/sw/lib/python") + +PPRZ_HOME = getenv("PAPARAZZI_HOME", PPRZ_SRC) + +from ivy_msg_interface import IvyMessagesInterface +from pprz_msg.message import PprzMessage WIDTH = 450 LABEL_WIDTH = 166 @@ -17,13 +22,41 @@ DATA_WIDTH = 100 HEIGHT = 800 BORDER = 1 -class MessagesFrame(wx.Frame): - def message_recv(self, ac_id, name, values): - if ac_id in self.aircrafts and name in self.aircrafts[ac_id].messages: - if time.time() - self.aircrafts[ac_id].messages[name].last_seen < 0.2: - return - wx.CallAfter(self.gui_update, ac_id, name, values) +class Message(PprzMessage): + def __init__(self, class_name, name): + super(Message, self).__init__(class_name, name) + self.field_controls = [] + self.index = None + self.last_seen = time.clock() + self.name = name + + +class Aircraft(object): + def __init__(self, ac_id): + self.ac_id = ac_id + self.messages = {} + self.messages_book = None + + +class MessagesFrame(wx.Frame): + def message_recv(self, ac_id, msg): + """Handle incoming messages + + Callback function for IvyMessagesInterface + + :param ac_id: aircraft id + :type ac_id: int + :param msg: message + :type msg: PprzMessage + """ + # only show messages of the requested class + if msg.get_classname() != self.msg_class: + return + if ac_id in self.aircrafts and msg.get_msgname() in self.aircrafts[ac_id].messages: + if time.time() - self.aircrafts[ac_id].messages[msg.get_msgname()].last_seen < 0.2: + return + wx.CallAfter(self.gui_update, msg.get_classname(), msg.get_msgname(), ac_id, msg) def find_page(self, book, name): if book.GetPageCount() < 1: @@ -31,11 +64,10 @@ class MessagesFrame(wx.Frame): start = 0 end = book.GetPageCount() - while (start < end): + while start < end: if book.GetPageText(start) > name: return start - start = start + 1 - + start += 1 return start def update_leds(self): @@ -53,7 +85,7 @@ class MessagesFrame(wx.Frame): self.timer.start() def setup_image_list(self, notebook): - imageList = wx.ImageList(24,24) + imageList = wx.ImageList(24, 24) image = wx.Image(PPRZ_HOME + "/data/pictures/gray_led24.png") bitmap = wx.BitmapFromImage(image) @@ -66,7 +98,7 @@ class MessagesFrame(wx.Frame): notebook.AssignImageList(imageList) def add_new_aircraft(self, ac_id): - self.aircrafts[ac_id] = messages_tool.Aircraft(ac_id) + self.aircrafts[ac_id] = Aircraft(ac_id) ac_panel = wx.Panel(self.notebook, -1) self.notebook.AddPage(ac_panel, str(ac_id)) messages_book = wx.Notebook(ac_panel, style=wx.NB_LEFT) @@ -77,21 +109,21 @@ class MessagesFrame(wx.Frame): sizer.Layout() self.aircrafts[ac_id].messages_book = messages_book - def add_new_message(self, aircraft, name): + def add_new_message(self, aircraft, msg_class, name): messages_book = aircraft.messages_book - aircraft.messages[name] = messages_tool.Message("telemetry", name) + aircraft.messages[name] = Message(msg_class, name) field_panel = wx.Panel(messages_book) - grid_sizer = wx.FlexGridSizer(len(aircraft.messages[name].field_names), 2) + grid_sizer = wx.FlexGridSizer(len(aircraft.messages[name].get_fieldnames()), 2) index = self.find_page(messages_book, name) - messages_book.InsertPage(index, field_panel, name, imageId = 1) + messages_book.InsertPage(index, field_panel, name, imageId=1) aircraft.messages[name].index = index # update indexes of pages which are to be moved for message_name in aircraft.messages: aircraft.messages[message_name].index = self.find_page(messages_book, message_name) - for field_name in aircraft.messages[name].field_names: + for field_name in aircraft.messages[name].get_fieldnames(): name_text = wx.StaticText(field_panel, -1, field_name) size = name_text.GetSize() size.x = LABEL_WIDTH @@ -109,22 +141,22 @@ class MessagesFrame(wx.Frame): field_panel.SetSizer(grid_sizer) field_panel.Layout() - def gui_update(self, ac_id, name, values): + def gui_update(self, msg_class, msg_name, ac_id, msg): if ac_id not in self.aircrafts: self.add_new_aircraft(ac_id) aircraft = self.aircrafts[ac_id] - if name not in aircraft.messages: - self.add_new_message(aircraft, name) + if msg_name not in aircraft.messages: + self.add_new_message(aircraft, msg_class, msg_name) - aircraft.messages_book.SetPageImage(aircraft.messages[name].index, 1) - self.aircrafts[ac_id].messages[name].last_seen = time.time() + aircraft.messages_book.SetPageImage(aircraft.messages[msg_name].index, 1) + self.aircrafts[ac_id].messages[msg_name].last_seen = time.time() - for index in range(0, len(values)): - aircraft.messages[name].field_controls[index].SetLabel(values[index]) + for index in range(0, len(msg.get_fieldvalues())): + aircraft.messages[msg_name].field_controls[index].SetLabel(msg.get_field(index)) - def __init__(self): + def __init__(self, msg_class="telemetry"): wx.Frame.__init__(self, id=-1, parent=None, name=u'MessagesFrame', size=wx.Size(WIDTH, HEIGHT), style=wx.DEFAULT_FRAME_STYLE, title=u'Messages') self.Bind(wx.EVT_CLOSE, self.OnClose) self.notebook = wx.Notebook(self) @@ -136,9 +168,10 @@ class MessagesFrame(wx.Frame): sizer.Layout() self.timer = threading.Timer(0.1, self.update_leds) self.timer.start() - self.interface = messages_tool.IvyMessagesInterface(self.message_recv) + self.msg_class = msg_class + self.interface = IvyMessagesInterface(self.message_recv) def OnClose(self, event): self.timer.cancel() - self.interface.Shutdown() + self.interface.shutdown() self.Destroy() diff --git a/sw/ground_segment/tmtc/messages.ml b/sw/ground_segment/tmtc/messages.ml index a4dfc95aee..4769e96a5a 100644 --- a/sw/ground_segment/tmtc/messages.ml +++ b/sw/ground_segment/tmtc/messages.ml @@ -41,7 +41,7 @@ let values_of_field = fun field -> _ -> [||] (** Display one page for a message *) -let one_page = fun sender class_name (notebook:GPack.notebook) (help_label:GObj.widget) (window:GWindow.window) bind m -> +let one_page = fun sender class_name (notebook:GPack.notebook) (topnote:GPack.notebook) (help_label:GObj.widget) (window:GWindow.window) bind m -> let id = (Xml.attrib m "name") in let h = GPack.hbox () in h#misc#set_property "name" (`STRING (Some id)); @@ -107,12 +107,12 @@ let one_page = fun sender class_name (notebook:GPack.notebook) (help_label:GObj. (* hide notebook and display help during drag *) let begin_drag = fun _ -> - notebook#coerce#misc#hide (); + topnote#coerce#misc#hide (); help_label#misc#show (); window#resize ~width:300 ~height:50 in ignore (field_label#drag#connect#beginning ~callback:begin_drag); - ignore (field_label#drag#connect#ending ~callback:(fun _ -> notebook#coerce#misc#show (); help_label#misc#hide ())); + ignore (field_label#drag#connect#ending ~callback:(fun _ -> topnote#coerce#misc#show (); help_label#misc#hide ())); (update, display_value)::rest with @@ -170,7 +170,7 @@ let one_page = fun sender class_name (notebook:GPack.notebook) (help_label:GObj. in bind id display -let rec one_class = fun (notebook:GPack.notebook) (help_label:GObj.widget) (window:GWindow.window) (ident, xml_class, sender) -> +let rec one_class = fun (notebook:GPack.notebook) (help_label:GObj.widget) (window:GWindow.window) timestamp force (ident, xml_class, sender) -> let class_name = (Xml.attrib xml_class "name") in let messages = Xml.children xml_class in let module P = Pprz.Messages (struct let name = class_name end) in @@ -181,23 +181,24 @@ let rec one_class = fun (notebook:GPack.notebook) (help_label:GObj.widget) (wind let get_one = fun sender _vs -> if not (Hashtbl.mem senders sender) then begin Hashtbl.add senders sender (); - one_class notebook help_label window (ident, xml_class, Some sender) + one_class notebook help_label window timestamp force (ident, xml_class, Some sender) end in - List.iter - (fun m -> ignore (P.message_bind (Xml.attrib m "name") get_one)) - messages + if force || not (class_name = "telemetry") then (* bind to all messages in class *) + List.iter (fun m -> ignore (P.message_bind ~timestamp (Xml.attrib m "name") get_one)) messages + else (* if telemetry and not forces, only wait for ALIVE message *) + ignore (P.message_bind ~timestamp "ALIVE" get_one) | _ -> let class_notebook = GPack.notebook ~tab_border:0 ~tab_pos:`LEFT () in let l = match sender with None -> "" | Some s -> ":"^s in let label = GMisc.label ~text:(ident^l) () in ignore (notebook#append_page ~tab_label:label#coerce class_notebook#coerce); let bind, sender_name = match sender with - None -> (fun m cb -> (P.message_bind m cb)), "*" - | Some sender -> (fun m cb -> (P.message_bind ~sender m cb)), sender in - + None -> (fun m cb -> (P.message_bind ~timestamp m cb)), "*" + | Some sender -> (fun m cb -> (P.message_bind ~sender ~timestamp m cb)), sender in + (** Forall messages in the class *) let messages = list_sort (fun x -> Xml.attrib x "name") messages in - List.iter (fun m -> ignore (one_page sender_name class_name class_notebook help_label window bind m)) messages + List.iter (fun m -> ignore (one_page sender_name class_name class_notebook notebook help_label window bind m)) messages @@ -206,9 +207,13 @@ let rec one_class = fun (notebook:GPack.notebook) (help_label:GObj.widget) (wind let _ = let ivy_bus = ref Defivybus.default_ivy_bus in let classes = ref ["telemetry:*"] in + let timestamp = ref false in + let force = ref false in Arg.parse [ "-b", Arg.String (fun x -> ivy_bus := x), (sprintf " Default is %s" !ivy_bus); - "-c", Arg.String (fun x -> classes := x :: !classes), "class name"] + "-c", Arg.String (fun x -> classes := x :: !classes), "class name"; + "-timestamp", Arg.Set timestamp, "Bind to timestampped messages"; + "-force", Arg.Set force, "Force waiting on all messages, not only ALIVE for telemetry class (increase network load)" ] (fun x -> prerr_endline ("WARNING: don't do anything with "^x)) "Usage: "; @@ -243,7 +248,7 @@ let _ = !classes in (* Insert the message classes in the notebook *) - List.iter (one_class notebook help_label#coerce window) xml_classes; + List.iter (one_class notebook help_label#coerce window !timestamp !force) xml_classes; (** Start the main loop *) window#show (); diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml index d4e0790ba3..65452e7772 100644 --- a/sw/ground_segment/tmtc/server.ml +++ b/sw/ground_segment/tmtc/server.ml @@ -622,14 +622,17 @@ let register_aircraft = fun name a -> (** Identifying message from an A/C *) -let ident_msg = fun log name vs -> +let ident_msg = fun log timestamp name vs -> try if not (Hashtbl.mem aircrafts name) && not (Hashtbl.mem unknown_aircrafts name) then let get_md5sum = fun () -> Pprz.assoc "md5sum" vs in let ac, messages_xml = new_aircraft get_md5sum name in let ac_msg_closure = ac_msg messages_xml log name ac in - let _b = Ivy.bind (fun _ args -> ac_msg_closure args.(1) args.(2)) (sprintf "^(([0-9]+\\.[0-9]+) )?%s +(.*)" name) in + let tsregexp = if timestamp then "(([0-9]+\\.[0-9]+) )?" else "" in + let _b = + Ivy.bind (fun _ args -> if timestamp then ac_msg_closure args.(1) args.(2) else ac_msg_closure "" args.(0)) + (sprintf "^%s%s +(.*)" tsregexp name) in register_aircraft name ac; Ground_Pprz.message_send my_id "NEW_AIRCRAFT" ["ac_id", Pprz.String name] with @@ -639,11 +642,11 @@ let new_color = fun () -> sprintf "#%02x%02x%02x" (Random.int 256) (Random.int 256) (Random.int 256) (* Waits for new aircrafts *) -let listen_acs = fun log -> +let listen_acs = fun log timestamp -> (** Wait for any message (they all are identified with the A/C) *) - ignore (Tm_Pprz.message_bind "ALIVE" (ident_msg log)); + ignore (Tm_Pprz.message_bind "ALIVE" (ident_msg log timestamp)); if !replay_old_log then - ignore (Tm_Pprz.message_bind "PPRZ_MODE" (ident_msg log)) + ignore (Tm_Pprz.message_bind "PPRZ_MODE" (ident_msg log timestamp)) let send_config = fun http _asker args -> @@ -785,7 +788,8 @@ let ground_to_uplink = fun logging -> let () = let ivy_bus = ref Defivybus.default_ivy_bus and logging = ref true - and http = ref false in + and http = ref false + and timestamp = ref false in let options = [ "-b", Arg.String (fun x -> ivy_bus := x), (sprintf "Bus\tDefault is %s" !ivy_bus); @@ -795,6 +799,7 @@ let () = "-kml_no_http", Arg.Set Kml.no_http, "KML without web server (local files only)"; "-kml_port", Arg.Set_int Kml.port, (sprintf "Port for KML files (default is %d)" !Kml.port); "-n", Arg.Clear logging, "Disable log"; + "-timestamp", Arg.Set timestamp, "Bind on timestampped messages"; "-no_md5_check", Arg.Set no_md5_check, "Disable safety matching of live and current configurations"; "-replay_old_log", Arg.Set replay_old_log, "Enable aircraft registering on PPRZ_MODE messages"] in @@ -816,7 +821,7 @@ let () = None in (* Waits for new aircrafts *) - listen_acs logging; + listen_acs logging !timestamp; (* Forward messages from ground agents to vehicles *) ground_to_uplink logging; diff --git a/sw/ground_segment/tmtc/server_globals.ml b/sw/ground_segment/tmtc/server_globals.ml index 32cbd0052d..6c7843a268 100644 --- a/sw/ground_segment/tmtc/server_globals.ml +++ b/sw/ground_segment/tmtc/server_globals.ml @@ -4,7 +4,7 @@ let hostname = ref "localhost" (** FIXME: Should be read from messages.xml *) let fixedwing_ap_modes = [|"MANUAL";"AUTO1";"AUTO2";"HOME";"NOGPS";"FAIL"|] -let rotorcraft_ap_modes = [|"KILL";"SAFE";"HOME";"RATE";"ATT";"R_RCC";"A_RCC";"ATT_C";"R_ZH";"A_ZH";"HOVER";"HOV_C";"H_ZH";"NAV";"RC_D";"CF";"FWD"|] +let rotorcraft_ap_modes = [|"KILL";"SAFE";"HOME";"RATE";"ATT";"R_RCC";"A_RCC";"ATT_C";"R_ZH";"A_ZH";"HOVER";"HOV_C";"H_ZH";"NAV";"RC_D";"CF";"FWD";"MODULE"|] let _AUTO2 = 2 let gaz_modes = [|"MANUAL";"THROTTLE";"CLIMB";"ALT"|] let lat_modes = [|"MANUAL";"ROLL_RATE";"ROLL";"COURSE"|] diff --git a/sw/ground_segment/tmtc/settings.ml b/sw/ground_segment/tmtc/settings.ml index 0569ccc69c..0daf24c917 100644 --- a/sw/ground_segment/tmtc/settings.ml +++ b/sw/ground_segment/tmtc/settings.ml @@ -55,7 +55,7 @@ let one_ac = fun (notebook:GPack.notebook) ac_name -> (* Build the buttons and sliders *) let xml = Xml.parse_file xml_file in let xmls = Xml.children (ExtXml.child xml "dl_settings") in - let settings = new Page_settings.settings xmls callback (fun _ _ -> ()) in + let settings = new Page_settings.settings xmls callback ac_id (fun _ _ -> ()) in (* Bind to values updates *) let get_dl_value = fun _sender vs -> diff --git a/sw/lib/ocaml/convert.c b/sw/lib/ocaml/convert.c index bcd07cccdd..3a16ec5c4c 100644 --- a/sw/lib/ocaml/convert.c +++ b/sw/lib/ocaml/convert.c @@ -27,14 +27,16 @@ #include #include #include -#include "caml/mlvalues.h" -#include "caml/alloc.h" -#include "inttypes.h" +#include +#include +#include +#include #ifdef ARCH_ALIGN_DOUBLE value c_float_of_indexed_bytes(value s, value index) { + CAMLparam2 (s, index); char *x = (char *)(String_val(s) + Int_val(index)); //Assert(sizeof(float) == 4); @@ -44,12 +46,13 @@ c_float_of_indexed_bytes(value s, value index) buffer.b[2] = x[2]; buffer.b[3] = x[3]; - return copy_double((double)buffer.f); + CAMLreturn (copy_double((double)buffer.f)); } value c_double_of_indexed_bytes(value s, value index) { + CAMLparam2 (s, index); char *x = (char *)(String_val(s) + Int_val(index)); //Assert(sizeof(double) == 8); @@ -58,110 +61,118 @@ c_double_of_indexed_bytes(value s, value index) for (i=0; i < sizeof(double); i++) { buffer.b[i] = x[i]; } - return copy_double(buffer.d); + CAMLreturn (copy_double(buffer.d)); } #else /* no ARCH_ALIGN_DOUBLE */ value c_float_of_indexed_bytes(value s, value index) { + CAMLparam2 (s, index); float *x = (float*)(String_val(s) + Int_val(index)); - - return copy_double((double)(*x)); + CAMLreturn (copy_double((double)(*x))); } value c_double_of_indexed_bytes(value s, value index) { + CAMLparam2 (s, index); double *x = (double*)(String_val(s) + Int_val(index)); - - return copy_double(*x); + CAMLreturn (copy_double(*x)); } #endif /* ARCH_ALIGN_DOUBLE */ value c_sprint_float(value s, value index, value f) { + CAMLparam3 (s, index, f); float *p = (float*) (String_val(s) + Int_val(index)); double ff = Double_val(f); *p = (float)ff; - return Val_unit; + CAMLreturn (Val_unit); } value c_sprint_double(value s, value index, value f) { + CAMLparam3 (s, index, f); double *p = (double*) (String_val(s) + Int_val(index)); *p = Double_val(f); - return Val_unit; + CAMLreturn (Val_unit); } value c_sprint_int16(value s, value index, value f) { + CAMLparam3 (s, index, f); int16_t *p = (int16_t*) (String_val(s) + Int_val(index)); *p = (int16_t)Int_val(f); - return Val_unit; + CAMLreturn (Val_unit); } value c_sprint_int8(value s, value index, value f) { + CAMLparam3 (s, index, f); int8_t *p = (int8_t*) (String_val(s) + Int_val(index)); *p = (int8_t)Int_val(f); - return Val_unit; + CAMLreturn (Val_unit); } value c_sprint_int32(value s, value index, value x) { + CAMLparam3 (s, index, x); int32_t *p = (int32_t*) (String_val(s) + Int_val(index)); *p = (int32_t)Int32_val(x); - return Val_unit; + CAMLreturn (Val_unit); } value c_sprint_int64(value s, value index, value x) { + CAMLparam3 (s, index, x); int64_t *p = (int64_t*) (String_val(s) + Int_val(index)); *p = (int64_t)Int64_val(x); - return Val_unit; + CAMLreturn (Val_unit); } value c_int16_of_indexed_bytes(value s, value index) { + CAMLparam2 (s, index); int16_t *x = (int16_t*)(String_val(s) + Int_val(index)); - - return Val_int(*x); + CAMLreturn (Val_int(*x)); } value c_int8_of_indexed_bytes(value s, value index) { + CAMLparam2 (s, index); int8_t *x = (int8_t*)(String_val(s) + Int_val(index)); - - return Val_int(*x); + CAMLreturn (Val_int(*x)); } value c_int32_of_indexed_bytes(value s, value index) { + CAMLparam2 (s, index); int32_t *x = (int32_t*)(String_val(s) + Int_val(index)); - - return copy_int32(*x); + CAMLreturn (copy_int32(*x)); } value c_uint32_of_indexed_bytes(value s, value index) { + CAMLparam2 (s, index); uint32_t *x = (uint32_t*)(String_val(s) + Int_val(index)); /* since OCaml doesn't have unsigned integers, * we represent it as 64bit signed int to make sure it doesn't overflow */ int64_t y = *x; - return copy_int64(y); + CAMLreturn (copy_int64(y)); } #ifdef ARCH_ALIGN_INT64 value c_int64_of_indexed_bytes(value s, value index) { + CAMLparam2 (s, index); char *x = (char *)(String_val(s) + Int_val(index)); union { char b[sizeof(int64_t)]; int64_t i; } buffer; @@ -169,14 +180,14 @@ c_int64_of_indexed_bytes(value s, value index) for (i=0; i < sizeof(int64_t); i++) { buffer.b[i] = x[i]; } - return copy_int64(buffer.i); + CAMLreturn (copy_int64(buffer.i)); } #else value c_int64_of_indexed_bytes(value s, value index) { + CAMLparam2 (s, index); int64_t *x = (int64_t*)(String_val(s) + Int_val(index)); - - return copy_int64(*x); + CAMLreturn (copy_int64(*x)); } #endif diff --git a/sw/lib/ocaml/cserial.c b/sw/lib/ocaml/cserial.c index 11f705f9b9..35f4ffba60 100644 --- a/sw/lib/ocaml/cserial.c +++ b/sw/lib/ocaml/cserial.c @@ -33,6 +33,7 @@ #include #include #include +#include static int baudrates[] = { B0, B50, B75, B110, B134, B150, B200, B300, B600, B1200, B1800, B2400, B4800, B9600, B19200, B38400, B57600, B115200, B230400 }; @@ -42,6 +43,7 @@ static int baudrates[] = { B0, B50, B75, B110, B134, B150, B200, B300, B600, B12 /****************************************************************************/ value c_init_serial(value device, value speed, value hw_flow_control) { + CAMLparam3 (device, speed, hw_flow_control); struct termios orig_termios, cur_termios; int br = baudrates[Int_val(speed)]; @@ -80,10 +82,12 @@ value c_init_serial(value device, value speed, value hw_flow_control) if (tcsetattr(fd, TCSADRAIN, &cur_termios)) failwith("setting modem serial device attr"); - return Val_int(fd); + CAMLreturn (Val_int(fd)); } -value c_set_dtr(value val_fd, value val_bit) { +value c_set_dtr(value val_fd, value val_bit) +{ + CAMLparam2 (val_fd, val_bit); int status; int fd = Int_val(val_fd); @@ -93,13 +97,14 @@ value c_set_dtr(value val_fd, value val_bit) { else status &= ~TIOCM_DTR; ioctl(fd, TIOCMSET, &status); - return Val_unit; + CAMLreturn (Val_unit); } /* From the gPhoto I/O library */ value c_serial_set_baudrate(value val_fd, value speed) { + CAMLparam2 (val_fd, speed); struct termios tio; int fd = Int_val(val_fd); @@ -121,5 +126,5 @@ value c_serial_set_baudrate(value val_fd, value speed) if (tcsetattr(fd, TCSANOW | TCSAFLUSH, &tio) < 0) { failwith("tcsetattr"); } - return Val_unit; + CAMLreturn (Val_unit); } diff --git a/sw/lib/ocaml/papget.ml b/sw/lib/ocaml/papget.ml index 8968fcb2bf..8bb305485e 100644 --- a/sw/lib/ocaml/papget.ml +++ b/sw/lib/ocaml/papget.ml @@ -64,7 +64,8 @@ object method connect = fun cb -> callbacks <- cb :: callbacks method config = fun () -> let field = sprintf "%s:%s" msg_name field_descr in - [ PC.property "field" field ] + let ac_id = match sender with None -> [] | Some id -> [PC.property "ac_id" id] in + [ PC.property "field" field ] @ ac_id method type_ = "message_field" initializer @@ -83,17 +84,17 @@ object end -let hash_vars = fun expr -> +let hash_vars = fun ?sender expr -> let htable = Hashtbl.create 3 in let rec loop = function - E.Ident i -> prerr_endline i + E.Ident i -> prerr_endline i | E.Int _ | E.Float _ -> () | E.Call (_id, list) | E.CallOperator (_id, list) -> List.iter loop list | E.Index (_id, e) -> loop e | E.Deref (_e, _f) as deref -> fprintf stderr "Warning: Deref operator is not allowed in Papgets expressions (%s)" (E.sprint deref) | E.Field (i, f) -> if not (Hashtbl.mem htable (i,f)) then - let msg_obj = new message_field i f in + let msg_obj = new message_field ?sender i f in Hashtbl.add htable (i, f) msg_obj in loop expr; htable @@ -111,7 +112,7 @@ let eval_bin_op = function let eval_expr = fun (extra_functions:(string * (string list -> string)) list) h e -> let rec loop = function - E.Ident ident -> failwith (sprintf "Papget.eval_expr '%s'" ident) + E.Ident ident -> failwith (sprintf "Papget.eval_expr '%s'" ident) | E.Int int -> string_of_int int | E.Float float -> string_of_float float | E.CallOperator (ident, [e1; e2]) -> @@ -131,8 +132,8 @@ let eval_expr = fun (extra_functions:(string * (string list -> string)) list) h -class expression = fun ?(extra_functions=[]) expr -> - let h = hash_vars expr in +class expression = fun ?(extra_functions=[]) ?sender expr -> + let h = hash_vars ?sender expr in object val mutable callbacks = [] val mutable last_value = "0." @@ -142,7 +143,8 @@ object method connect = fun cb -> callbacks <- cb :: callbacks method config = fun () -> - [ PC.property "expr" (Expr_syntax.sprint expr)] + let ac_id = match sender with None -> [] | Some id -> [PC.property "ac_id" id] in + [ PC.property "expr" (Expr_syntax.sprint expr)] @ ac_id method type_ = "expression" @@ -229,8 +231,16 @@ object (self) | `BUTTON_RELEASE ev -> if GdkEvent.Button.button ev = 1 then begin item#ungrab (GdkEvent.Button.time ev); + (* get item and window size *) + let bounds = item#get_bounds in + let w, h = Gdk.Drawable.get_size item#canvas#misc#window in if not motion then begin self#edit () + end + else if (truncate bounds.(0) > w) || (truncate bounds.(2) < 0) || (truncate bounds.(1) > h) || (truncate bounds.(3) < 0) then begin + (* delete an item if placed out of the window on the left or top side *) + item#destroy (); + deleted <- true end; motion <- false end; @@ -241,6 +251,9 @@ object (self) let file = Env.paparazzi_src // "sw" // "lib" // "ocaml" // "widgets.glade" in let dialog = new Gtk_papget_editor.papget_editor ~file () in + let ac_id = PC.get_prop "ac_id" config "Any" in + dialog#toplevel#set_title ("Papget Editor (A/C: "^ac_id^")"); + let tagged_renderers = Lazy.force PR.lazy_tagged_renderers in let strings = List.map fst tagged_renderers in diff --git a/sw/lib/ocaml/papget.mli b/sw/lib/ocaml/papget.mli index 21b1115566..1ea06e8c1a 100644 --- a/sw/lib/ocaml/papget.mli +++ b/sw/lib/ocaml/papget.mli @@ -45,6 +45,7 @@ class message_field : class expression : ?extra_functions:(string * (string list -> string)) list -> + ?sender:string -> Expr_syntax.expression -> value diff --git a/sw/lib/ocaml/papget_renderer.ml b/sw/lib/ocaml/papget_renderer.ml index e2e01e16a0..6bc949e25f 100644 --- a/sw/lib/ocaml/papget_renderer.ml +++ b/sw/lib/ocaml/papget_renderer.ml @@ -171,9 +171,9 @@ class canvas_gauge = fun ?(config=[]) canvas_group x y -> (*let props = (text_props@[`ANCHOR `EAST]) in*) let _ = GnoCanvas.ellipse ~x1:r2 ~y1:r2 ~x2:(-.r2) ~y2:(-.r2) - ~props:[`NO_FILL_COLOR; `OUTLINE_COLOR "grey"; `WIDTH_UNITS 6.] root in + ~props:[`NO_FILL_COLOR; `OUTLINE_COLOR "grey"; `WIDTH_PIXELS 6] root in let points = [|0.;-.r1;0.;-.r1+.3.|] in - let props = [`WIDTH_UNITS 2.; `FILL_COLOR "red"] in + let props = [`WIDTH_PIXELS 2; `FILL_COLOR "red"] in let _ = GnoCanvas.line ~points ~props root in let il = GnoCanvas.line ~points ~props root in let () = il#affine_absolute (affine_pos_and_angle 0. 0. (-. Latlong.pi /. 3.)) in diff --git a/sw/lib/ocaml/pprz.ml b/sw/lib/ocaml/pprz.ml index a2146129f8..42661ef69e 100644 --- a/sw/lib/ocaml/pprz.ml +++ b/sw/lib/ocaml/pprz.ml @@ -613,7 +613,7 @@ module type MESSAGES = sig val message_send : ?timestamp:float -> ?link_id:int -> string -> string -> values -> unit (** [message_send sender link_id msg_name values] *) - val message_bind : ?sender:string -> string -> (string -> values -> unit) -> Ivy.binding + val message_bind : ?sender:string -> ?timestamp:bool -> string -> (string -> values -> unit) -> Ivy.binding (** [message_bind ?sender msg_name callback] *) val message_answerer : string -> string -> (string -> values -> values) -> Ivy.binding @@ -764,20 +764,21 @@ module MessagesOfXml(Class:CLASS_Xml) = struct Ivy.send ( Printf.sprintf "redlink TELEMETRY_MESSAGE %s %i %s" sender the_link_id modified_msg); end - let message_bind = fun ?sender msg_name cb -> + let message_bind = fun ?sender ?(timestamp=false) msg_name cb -> + let tsregexp, tsoffset = if timestamp then "([0-9]+\\.[0-9]+ )?", 1 else "", 0 in match sender with None -> Ivy.bind (fun _ args -> - let values = try snd (values_of_string args.(2)) with exc -> prerr_endline (Printexc.to_string exc); [] in - cb args.(1) values) - (sprintf "^([0-9]+\\.[0-9]+ )?([^ ]*) +(%s( .*|$))" msg_name) + let values = try snd (values_of_string args.(1+tsoffset)) with exc -> prerr_endline (Printexc.to_string exc); [] in + cb args.(tsoffset) values) + (sprintf "^%s([^ ]*) +(%s( .*|$))" tsregexp msg_name) | Some s -> Ivy.bind (fun _ args -> - let values = try snd (values_of_string args.(1)) with exc -> prerr_endline (Printexc.to_string exc); [] in + let values = try snd (values_of_string args.(tsoffset)) with exc -> prerr_endline (Printexc.to_string exc); [] in cb s values) - (sprintf "^([0-9]+\\.[0-9]+ )?%s +(%s( .*|$))" s msg_name) + (sprintf "^%s%s +(%s( .*|$))" tsregexp s msg_name) let message_answerer = fun sender msg_name cb -> let ivy_cb = fun _ args -> diff --git a/sw/lib/ocaml/pprz.mli b/sw/lib/ocaml/pprz.mli index eef75f694e..31887a6d07 100644 --- a/sw/lib/ocaml/pprz.mli +++ b/sw/lib/ocaml/pprz.mli @@ -179,7 +179,7 @@ module type MESSAGES = sig val message_send : ?timestamp:float -> ?link_id:int -> string -> string -> values -> unit (** [message_send sender msg_name values] *) - val message_bind : ?sender:string ->string -> (string -> values -> unit) -> Ivy.binding + val message_bind : ?sender:string -> ?timestamp:bool -> string -> (string -> values -> unit) -> Ivy.binding (** [message_bind ?sender msg_name callback] *) val message_answerer : string -> string -> (string -> values -> values) -> Ivy.binding diff --git a/sw/lib/ocaml/widgets.glade b/sw/lib/ocaml/widgets.glade index 3d3e9bbf25..bf111e9a43 100644 --- a/sw/lib/ocaml/widgets.glade +++ b/sw/lib/ocaml/widgets.glade @@ -102,8 +102,9 @@ white True - Papget Editor + Papget Editor (A/C: Any) True + 300 True diff --git a/sw/lib/python/ivy_msg_interface.py b/sw/lib/python/ivy_msg_interface.py new file mode 100644 index 0000000000..245f16d91a --- /dev/null +++ b/sw/lib/python/ivy_msg_interface.py @@ -0,0 +1,90 @@ +from __future__ import absolute_import, print_function + +from ivy.std_api import * +import logging +import os +import sys +import re + +# if PAPARAZZI_SRC not set, then assume the tree containing this +# file is a reasonable substitute +PPRZ_SRC = os.getenv("PAPARAZZI_SRC", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), + '../../../../'))) +sys.path.append(PPRZ_SRC + "/sw/lib/python") + +from pprz_msg.message import PprzMessage + + +class IvyMessagesInterface(object): + def __init__(self, callback, init=True, verbose=False, bind_regex='(.*)'): + self.callback = callback + self.ivy_id = 0 + self.verbose = verbose + self.init_ivy(init, bind_regex) + + def stop(self): + IvyUnBindMsg(self.ivy_id) + + def shutdown(self): + self.stop() + IvyStop() + + def __init__del__(self): + try: + IvyUnBindMsg(self.ivy_id) + except: + pass + + def init_ivy(self, init=True, bind_regex='(.*)'): + if init: + IvyInit("Messages %i" % os.getpid(), "READY", 0, lambda x,y: y, lambda x,y: y) + logging.getLogger('Ivy').setLevel(logging.WARN) + IvyStart("") + self.ivy_id = IvyBindMsg(self.on_ivy_msg, bind_regex) + + def on_ivy_msg(self, agent, *larg): + """ Split ivy message up into the separate parts + Basically parts/args in string are separated by space, but char array can also contain a space: + |f,o,o, ,b,a,r| in old format or "foo bar" in new format + """ + # first split on array delimiters + l = re.split('([|\"][^|]*[|\"])', larg[0]) + # strip spaces and filter out emtpy strings + l = [str.strip(s) for s in l if str.strip(s) is not ''] + data = [] + for s in l: + # split non-array strings further up + if '|' not in s and '"' not in s: + data += s.split(' ') + else: + data.append(s) + # ignore ivy message with less than 3 elements + if len(data) < 3: + return + + # check which message class it is + # pass non-telemetry messages with ac_id 0 + if data[0] in ["sim", "ground_dl", "dl"]: + if self.verbose: + print("ignoring message " + ' '.join(data)) + sys.stdout.flush() + return + elif data[0] in ["ground"]: + msg_class = data[0] + msg_name = data[1] + ac_id = 0 + values = list(filter(None, data[2:])) + else: + try: + ac_id = int(data[0]) + except ValueError: + if self.verbose: + print("ignoring message " + ' '.join(data)) + sys.stdout.flush() + return + msg_class = "telemetry" + msg_name = data[1] + values = list(filter(None, data[2:])) + msg = PprzMessage(msg_class, msg_name) + msg.set_values(values) + self.callback(ac_id, msg) diff --git a/sw/lib/python/messages_tool.py b/sw/lib/python/messages_tool.py deleted file mode 100644 index 7eaa067024..0000000000 --- a/sw/lib/python/messages_tool.py +++ /dev/null @@ -1,84 +0,0 @@ -from __future__ import absolute_import, print_function - -import messages_xml_map -from ivy.std_api import * -import logging -import time -import os -import re - -class Message: - def __init__(self, class_name, name): - messages_xml_map.ParseMessages() - self.field_value = [] - self.field_names = messages_xml_map.message_dictionary[class_name][name] - self.field_controls = [] - self.index = None - self.last_seen = time.clock() - self.name = name - -class Aircraft: - def __init__(self, id): - self.ac_id = id - self.messages = {} - self.messages_book = None - -class IvyMessagesInterface(): - def __init__(self, callback, initIvy = True): - self.callback = callback - self.ivy_id = 0 - self.InitIvy(initIvy) - - def Stop(self): - IvyUnBindMsg(self.ivy_id) - - def Shutdown(self): - self.Stop() - IvyStop() - - def __init__del__(self): - try: - IvyUnBindMsg(self.ivy_id) - except: - pass - - def InitIvy(self, initIvy): - if initIvy: - IvyInit("Messages %i" % os.getpid(), "READY", 0, lambda x,y: y, lambda x,y: y) - logging.getLogger('Ivy').setLevel(logging.WARN) - IvyStart("") - self.ivy_id = IvyBindMsg(self.OnIvyMsg, "(.*)") - - def OnIvyMsg(self, agent, *larg): - """ Split ivy message up into the separate parts - Basically parts/args in string are separated by space, but char array can also contain a space: - |f,o,o, ,b,a,r| in old format or "foo bar" in new format - """ - # first split on array delimiters - l = re.split('([|\"][^|]*[|\"])', larg[0]) - # strip spaces and filter out emtpy strings - l = [str.strip(s) for s in l if str.strip(s) is not ''] - data = [] - for s in l: - # split non-array strings further up - if '|' not in s and '"' not in s: - data += s.split(' ') - else: - data.append(s) - try: - ac_id = int(data[0]) - name = data[1] - values = list(filter(None, data[2:])) - self.callback(ac_id, name, values) - except ValueError: - pass - except: - raise - -def test(): - message = Message("WHIRLY") - print(message) - print(message.field_names) - -if __name__ == '__main__': - test() diff --git a/sw/lib/python/messages_xml_map.py b/sw/lib/python/messages_xml_map.py deleted file mode 100755 index 9cf660677f..0000000000 --- a/sw/lib/python/messages_xml_map.py +++ /dev/null @@ -1,78 +0,0 @@ -#!/usr/bin/env python - -from __future__ import absolute_import, print_function - -import os -import sys -import getopt - -messages_path = '%s/conf/messages.xml' % os.getenv("PAPARAZZI_HOME") - -message_dictionary = {} -message_dictionary_types = {} -message_dictionary_id_name = {} -message_dictionary_name_id = {} - -def Usage(scmd): - lpathitem = scmd.split('/') - fmt = '''Usage: %s [-h | --help] [-f FILE | --file=FILE] -where -\t-h | --help print this message -\t-f FILE | --file=FILE where FILE is path to messages.xml -''' - print(fmt % lpathitem[-1]) - -def GetOptions(): - try: - optlist, left_args = getopt.getopt(sys.argv[1:],'hf:', ['help','file=']) - except getopt.GetoptError: - # print help information and exit: - Usage(sys.argv[0]) - sys.exit(2) - for o, a in optlist: - if o in ("-h", "--help"): - Usage(sys.argv[0]) - sys.exit() - elif o in ("-f", "--file"): - messages_path = a - - -def ParseMessages(): - from lxml import etree - tree = etree.parse( messages_path) - for the_class in tree.xpath("//msg_class[@name]"): - class_name = the_class.attrib['name'] - if class_name not in message_dictionary: - message_dictionary_id_name[class_name] = {} - message_dictionary_name_id[class_name] = {} - message_dictionary[class_name] = {} - message_dictionary_types[class_name] = {} - for the_message in the_class.xpath("message[@name]"): - message_name = the_message.attrib['name'] - if 'id' in the_message.attrib: - message_id = the_message.attrib['id'] - else: - message_id = the_message.attrib['ID'] - if (message_id[0:2] == "0x"): - message_id = int(message_id, 16) - else: - message_id = int(message_id) - - message_dictionary_id_name[class_name][message_id] = message_name - message_dictionary_name_id[class_name][message_name] = message_id - - # insert this message into our dictionary as a list with room for the fields - message_dictionary[class_name][message_name] = [] - message_dictionary_types[class_name][message_id] = [] - - for the_field in the_message.xpath('field[@name]'): - # for now, just save the field names -- in the future maybe expand this to save a struct? - message_dictionary[class_name][message_name].append( the_field.attrib['name']) - message_dictionary_types[class_name][message_id].append( the_field.attrib['type']) - -def test(): - GetOptions() - ParseMessages() - -if __name__ == '__main__': - test() diff --git a/sw/lib/python/pprz_msg/__init__.py b/sw/lib/python/pprz_msg/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sw/lib/python/pprz_msg/message.py b/sw/lib/python/pprz_msg/message.py new file mode 100644 index 0000000000..2c81411488 --- /dev/null +++ b/sw/lib/python/pprz_msg/message.py @@ -0,0 +1,68 @@ +""" +Paparazzi message representation + +""" + +from __future__ import print_function +import sys +import json +import messages_xml_map + + +class PprzMessageError(Exception): + def __init__(self, message, inner_exception=None): + self.message = message + self.inner_exception = inner_exception + self.exception_info = sys.exc_info() + def __str__(self): + return self.message + + +class PprzMessage(object): + """base Paparazzi message class""" + def __init__(self, class_name, name): + self._class_name = class_name + self._name = name + self._id = messages_xml_map.get_msg_id(class_name, name) + self._fieldnames = messages_xml_map.get_msg_fields(class_name, name) + self._fieldvalues = [] + + def get_msgname(self): + return self._name + + def get_classname(self): + return self._class_name + + def get_fieldnames(self): + return self._fieldnames + + def get_fieldvalues(self): + return self._fieldvalues + + def get_field(self, idx): + return self._fieldvalues[idx] + + def set_values(self, values): + if len(values) == len(self._fieldnames): + self._fieldvalues = values + else: + raise PprzMessageError("Error: fields not matching") + + def __str__(self): + ret = '%s.%s {' % (self._class_name, self._name) + for idx, f in enumerate(self._fieldnames): + ret += '%s : %s, ' % (f, self._fieldvalues[idx]) + ret = ret[0:-2] + '}' + return ret + + def to_dict(self, payload_only=False): + d = {} + if not payload_only: + d['msgname'] = self._name + d['msgclass'] = self._class_name + for idx, f in enumerate(self._fieldnames): + d[f] = self._fieldvalues[idx] + return d + + def to_json(self, payload_only=False): + return json.dumps(self.to_dict(payload_only)) diff --git a/sw/lib/python/pprz_msg/messages_xml_map.py b/sw/lib/python/pprz_msg/messages_xml_map.py new file mode 100755 index 0000000000..1a8cdb1294 --- /dev/null +++ b/sw/lib/python/pprz_msg/messages_xml_map.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python + +from __future__ import absolute_import, print_function + +import os + +# if PAPARAZZI_HOME not set, then assume the tree containing this +# file is a reasonable substitute +PPRZ_HOME = os.getenv("PAPARAZZI_HOME", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), + '../../../..'))) + +default_messages_file = '%s/conf/messages.xml' % PPRZ_HOME + +message_dictionary = {} +message_dictionary_types = {} +message_dictionary_id_name = {} +message_dictionary_name_id = {} + + +class MessagesNotFound(Exception): + def __init__(self, filename): + self.filename = filename + + def __str__(self): + return "messages file " + repr(self.filename) + " not found" + + +def parse_messages(messages_file=default_messages_file): + if not os.path.isfile(messages_file): + raise MessagesNotFound(messages_file) + from lxml import etree + tree = etree.parse(messages_file) + for the_class in tree.xpath("//msg_class[@name]"): + class_name = the_class.attrib['name'] + if class_name not in message_dictionary: + message_dictionary_id_name[class_name] = {} + message_dictionary_name_id[class_name] = {} + message_dictionary[class_name] = {} + message_dictionary_types[class_name] = {} + for the_message in the_class.xpath("message[@name]"): + message_name = the_message.attrib['name'] + if 'id' in the_message.attrib: + message_id = the_message.attrib['id'] + else: + message_id = the_message.attrib['ID'] + if message_id[0:2] == "0x": + message_id = int(message_id, 16) + else: + message_id = int(message_id) + + message_dictionary_id_name[class_name][message_id] = message_name + message_dictionary_name_id[class_name][message_name] = message_id + + # insert this message into our dictionary as a list with room for the fields + message_dictionary[class_name][message_name] = [] + message_dictionary_types[class_name][message_id] = [] + + for the_field in the_message.xpath('field[@name]'): + # for now, just save the field names -- in the future maybe expand this to save a struct? + message_dictionary[class_name][message_name].append(the_field.attrib['name']) + message_dictionary_types[class_name][message_id].append(the_field.attrib['type']) + + +def get_msg_fields(msg_class, msg_name): + if not message_dictionary: + parse_messages() + if msg_class in message_dictionary: + if msg_name in message_dictionary[msg_class]: + return message_dictionary[msg_class][msg_name] + else: + print("Error: msg_name %s not found in msg_class %s." % (msg_name, msg_class)) + else: + print("Error: msg_class %s not found." % msg_class) + return [] + + +def get_msg_id(msg_class, msg_name): + if not message_dictionary: + parse_messages() + try: + return message_dictionary_name_id[msg_class][msg_name] + except KeyError: + print("Error: msg_name %s not found in msg_class %s." % (msg_name, msg_class)) + return 0 + + + +def test(): + import argparse + parser = argparse.ArgumentParser() + parser.add_argument("-f", "--file", help="path to messages.xml file", default=default_messages_file) + parser.add_argument("-l", "--list", help="list parsed messages", action="store_true", dest="list_messages") + parser.add_argument("-c", "--class", help="message class", dest="msg_class", default="telemetry") + args = parser.parse_args() + parse_messages(args.file) + if args.list_messages: + print("Listing %i messages in '%s' msg_class" % (len(message_dictionary[args.msg_class]), args.msg_class)) + for msg in message_dictionary[args.msg_class]: + print(msg) + +if __name__ == '__main__': + test() diff --git a/sw/simulator/nps/nps_autopilot_fixedwing.c b/sw/simulator/nps/nps_autopilot_fixedwing.c index 9e0c3e726b..399bd606fc 100644 --- a/sw/simulator/nps/nps_autopilot_fixedwing.c +++ b/sw/simulator/nps/nps_autopilot_fixedwing.c @@ -114,7 +114,7 @@ void nps_autopilot_run_step(double time) { if (nps_sensors_baro_available()) { float pressure = (float) sensors.baro.value; - AbiSendMsgBARO_ABS(BARO_SIM_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_SIM_SENDER_ID, pressure); Fbw(event_task); Ap(event_task); } diff --git a/sw/simulator/nps/nps_autopilot_rotorcraft.c b/sw/simulator/nps/nps_autopilot_rotorcraft.c index 46bfe2a5f7..d93dd90b88 100644 --- a/sw/simulator/nps/nps_autopilot_rotorcraft.c +++ b/sw/simulator/nps/nps_autopilot_rotorcraft.c @@ -99,14 +99,14 @@ void nps_autopilot_run_step(double time) { if (nps_sensors_baro_available()) { float pressure = (float) sensors.baro.value; - AbiSendMsgBARO_ABS(BARO_SIM_SENDER_ID, &pressure); + AbiSendMsgBARO_ABS(BARO_SIM_SENDER_ID, pressure); main_event(); } #if USE_SONAR if (nps_sensors_sonar_available()) { float dist = (float) sensors.sonar.value; - AbiSendMsgAGL(AGL_SONAR_NPS_ID, &dist); + AbiSendMsgAGL(AGL_SONAR_NPS_ID, dist); uint16_t foo = 0; DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &foo, &dist); diff --git a/sw/tools/generators/gen_abi.ml b/sw/tools/generators/gen_abi.ml index 8f4763bfc6..a456a6d62e 100644 --- a/sw/tools/generators/gen_abi.ml +++ b/sw/tools/generators/gen_abi.ml @@ -99,8 +99,8 @@ module Gen_onboard = struct let rec args = fun h l -> match l with [] -> Printf.fprintf h ")" - | [(n,t)] -> Printf.fprintf h ", const %s * %s)" t n - | (n,t)::l' -> Printf.fprintf h ", const %s * %s" t n; args h l' + | [(n,t)] -> Printf.fprintf h ", %s %s)" t n + | (n,t)::l' -> Printf.fprintf h ", %s %s" t n; args h l' in Printf.fprintf h "(uint8_t sender_id"; args h fields diff --git a/sw/tools/generators/gen_flight_plan.ml b/sw/tools/generators/gen_flight_plan.ml index a1ce64e7f3..93e9d1850c 100644 --- a/sw/tools/generators/gen_flight_plan.ml +++ b/sw/tools/generators/gen_flight_plan.ml @@ -144,6 +144,11 @@ let print_waypoint_lla_wgs84 = fun utm0 default_alt waypoint -> let alt = float_of_string alt +. Egm96.of_wgs84 wgs84 in printf " {.lat=%Ld, .lon=%Ld, .alt=%.0f}, /* 1e7deg, 1e7deg, mm (above WGS84 ref ellipsoid) */ \\\n" (convert_angle wgs84.posn_lat) (convert_angle wgs84.posn_long) (1000. *. alt) +let print_waypoint_global = fun waypoint -> + try + let (_, _) = (float_attrib waypoint "lat", float_attrib waypoint "lon") in + printf " TRUE, \\\n" + with _ -> printf " FALSE, \\\n" let get_index_block = fun x -> try @@ -848,6 +853,9 @@ let () = Xml2h.define "WAYPOINTS_LLA_WGS84" "{ \\"; List.iter (print_waypoint_lla_wgs84 utm0 alt) waypoints; lprintf "};\n"; + Xml2h.define "WAYPOINTS_GLOBAL" "{ \\"; + List.iter print_waypoint_global waypoints; + lprintf "};\n"; Xml2h.define "NB_WAYPOINT" (string_of_int (List.length waypoints)); Xml2h.define "FP_BLOCKS" "{ \\";