From c84c5708e5e76deba94d0be3112574fbbbfe7efc Mon Sep 17 00:00:00 2001 From: dewagter Date: Wed, 7 Jan 2015 13:50:48 +0100 Subject: [PATCH 01/99] move computervision to modules --- .../OpticFlow/OpticFlowHover.xml | 44 + .../modules/computer_vision/OpticFlow/dummy.c | 3 + .../OpticFlow/hover_stabilization.c | 166 + .../OpticFlow/hover_stabilization.h | 46 + .../OpticFlow/opticflow_code.c | 332 + .../OpticFlow/opticflow_code.h | 49 + .../OpticFlow/opticflow_module.c | 218 + .../OpticFlow/opticflow_module.h | 45 + .../OpticFlow/video_message_structs.h | 32 + .../modules/computer_vision/cv/color.h | 6 +- .../computer_vision/cv/encoding/jpeg.c | 3 +- .../modules/computer_vision/cv/encoding/rtp.c | 16 +- .../cv/opticflow/fast9/LICENSE | 30 + .../cv/opticflow/fast9/fastRosten.c | 6069 +++++++++++++++++ .../cv/opticflow/fast9/fastRosten.h | 51 + .../cv/opticflow/optic_flow_ardrone.c | 624 ++ .../cv/opticflow/optic_flow_ardrone.h | 43 + .../modules/computer_vision/cv/resize.h | 6 +- sw/airborne/modules/computer_vision/cv/trig.c | 158 + sw/airborne/modules/computer_vision/cv/trig.h | 7 + .../modules/computer_vision/lib/paparazzi.h | 32 + .../modules/computer_vision/lib/tcp/socket.c | 139 + .../modules/computer_vision/lib/tcp/socket.h | 17 + .../modules/computer_vision/lib/udp/socket.c | 3 +- .../computer_vision/lib/v4l/ardrone2.c | 0 .../computer_vision/lib/v4l/ardrone2.h | 0 .../modules/computer_vision/lib/v4l/video.c | 49 +- .../modules/computer_vision/lib/v4l/video.h | 4 +- 28 files changed, 8143 insertions(+), 49 deletions(-) create mode 100644 sw/airborne/modules/computer_vision/OpticFlow/OpticFlowHover.xml create mode 100644 sw/airborne/modules/computer_vision/OpticFlow/dummy.c create mode 100644 sw/airborne/modules/computer_vision/OpticFlow/hover_stabilization.c create mode 100644 sw/airborne/modules/computer_vision/OpticFlow/hover_stabilization.h create mode 100644 sw/airborne/modules/computer_vision/OpticFlow/opticflow_code.c create mode 100644 sw/airborne/modules/computer_vision/OpticFlow/opticflow_code.h create mode 100644 sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.c create mode 100644 sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.h create mode 100644 sw/airborne/modules/computer_vision/OpticFlow/video_message_structs.h create mode 100644 sw/airborne/modules/computer_vision/cv/opticflow/fast9/LICENSE create mode 100644 sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.c create mode 100644 sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.h create mode 100644 sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.c create mode 100644 sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.h create mode 100644 sw/airborne/modules/computer_vision/cv/trig.c create mode 100644 sw/airborne/modules/computer_vision/cv/trig.h create mode 100644 sw/airborne/modules/computer_vision/lib/paparazzi.h create mode 100644 sw/airborne/modules/computer_vision/lib/tcp/socket.c create mode 100644 sw/airborne/modules/computer_vision/lib/tcp/socket.h create mode 100644 sw/airborne/modules/computer_vision/lib/v4l/ardrone2.c create mode 100644 sw/airborne/modules/computer_vision/lib/v4l/ardrone2.h diff --git a/sw/airborne/modules/computer_vision/OpticFlow/OpticFlowHover.xml b/sw/airborne/modules/computer_vision/OpticFlow/OpticFlowHover.xml new file mode 100644 index 0000000000..3614379eb9 --- /dev/null +++ b/sw/airborne/modules/computer_vision/OpticFlow/OpticFlowHover.xml @@ -0,0 +1,44 @@ + + + + + Video ARDone 2 + + +
+ +
+ + + + + + + + + +include $(PAPARAZZI_HOME)/sw/ext/ardrone2_vision/Makefile.paths + +VISION_MODULE_FOLDER = $(DIR_MODULES)/OpticFlow + +$(TARGET).CFLAGS += -I$(DIR_MODULES) -I$(DIR_CV) -I$(DIR_LIB) -pthread -D__USE_GNU +$(TARGET).CXXFLAGS += -I$(PAPARAZZI_HOME)/sw/include/ -I$(PAPARAZZI_SRC)/sw/airborne -I$(PAPARAZZI_SRC)/conf/autopilot -I$(PAPARAZZI_SRC)/sw/airborne/arch/$($(TARGET).ARCHDIR) -I$(VARINCLUDE) -I$(ACINCLUDE) -I$(PAPARAZZI_SRC)/sw/airborne/modules/ +$(TARGET).CXXFLAGS += -I$(DIR_MODULES) -I$(DIR_CV) -I$(DIR_LIB) -pthread -D__USE_GNU + +$(TARGET).srcs += $(VISION_MODULE_FOLDER)/opticflow_module.c +$(TARGET).srcs += $(VISION_MODULE_FOLDER)/opticflow_code.c +$(TARGET).srcs += $(VISION_MODULE_FOLDER)/hover_stabilization.c +$(TARGET).srcs += $(DIR_CV)/opticflow/optic_flow_ardrone.c +$(TARGET).srcs += $(DIR_CV)/opticflow/fast9/fastRosten.c +$(TARGET).srcs += $(DIR_CV)/encoding/jpeg.c +$(TARGET).srcs += $(DIR_CV)/encoding/rtp.c +$(TARGET).srcs += $(DIR_CV)/trig.c +$(TARGET).srcs += $(DIR_LIB)/udp/socket.c +$(TARGET).srcs += $(DIR_LIB)/v4l/video.c +$(TARGET).CFLAGS += -I$(DIR_MODULES) -I$(DIR_CV) -I$(DIR_LIB) -pthread +$(TARGET).LDFLAGS += -pthread -lrt -static + + + + + diff --git a/sw/airborne/modules/computer_vision/OpticFlow/dummy.c b/sw/airborne/modules/computer_vision/OpticFlow/dummy.c new file mode 100644 index 0000000000..63978a0e13 --- /dev/null +++ b/sw/airborne/modules/computer_vision/OpticFlow/dummy.c @@ -0,0 +1,3 @@ +void dummyFunction(void) { + return; +} 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..0acb8b7674 --- /dev/null +++ b/sw/airborne/modules/computer_vision/OpticFlow/hover_stabilization.c @@ -0,0 +1,166 @@ +/* + * 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, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/* + * @file paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/hover_stabilization.c + * @brief optical-flow based hovering for Parrot AR.Drone 2.0 + * + * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 + */ + +// Own Header +#include "hover_stabilization.h" + +// Vision Data +#include "opticflow_code.h" + +// Stabilization +//#include "stabilization.h" +#include "firmwares/rotorcraft/stabilization/stabilization_attitude.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; + +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(void) +{ + if(autopilot_mode == AP_MODE_VISION_HOVER) + { + run_opticflow_hover(); + } + else + { + Velx_Int = 0; + Vely_Int = 0; + } +} + +void run_opticflow_hover(void) +{ + if(flow_count) + { + Error_Velx = Velx - vision_desired_vx; + Error_Vely = 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, &Velx, &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..a0a69dda9c --- /dev/null +++ b/sw/airborne/modules/computer_vision/OpticFlow/hover_stabilization.h @@ -0,0 +1,46 @@ +/* + * 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, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/* + * @file paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/hover_stabilization.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 HOVER_STABILIZATION_H_ +#define HOVER_STABILIZATION_H_ + +#include + +void init_hover_stabilization_onvision(void); +void run_hover_stabilization_onvision(void); +void run_opticflow_hover(void); + +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/opticflow_code.c b/sw/airborne/modules/computer_vision/OpticFlow/opticflow_code.c new file mode 100644 index 0000000000..bac9bfc338 --- /dev/null +++ b/sw/airborne/modules/computer_vision/OpticFlow/opticflow_code.c @@ -0,0 +1,332 @@ +/* + * 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, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/* + * @file paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/opticflow_code.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 +#include + +// Own Header +#include "opticflow_code.h" + +// Computer Vision +#include "opticflow/optic_flow_gdc.h" +#include "opticflow/fast9/fastRosten.h" +#include "opticflow_module.h" + +// Paparazzi Data +#include "subsystems/ins/ins_int.h" +#include "subsystems/imu.h" + +// Downlink +#include "subsystems/datalink/downlink.h" + +// Timer +#include + +// Settable by plugin +unsigned int imgWidth, imgHeight; +unsigned int verbose = 0; + +// Local variables +unsigned char *prev_frame, *gray_frame, *prev_gray_frame; +int old_img_init; +float OFx, OFy, dx_sum, dy_sum; + +// 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 +int *x, *y; +int count = 0; +int max_count = 25; +#define MAX_COUNT 100 + +// Corner Tracking +int *new_x, *new_y, *status, *dx, *dy; +int error_opticflow; +int flow_count = 0; +int remove_point; +int c; +int borderx = 24, bordery = 24; + +// Remove bad corners +float distance2, min_distance, min_distance2; + +// Flow Derotation +#define FLOW_DEROTATION +float curr_pitch, curr_roll, curr_yaw, prev_pitch, prev_roll; +float cam_h, diff_roll, diff_pitch, OFx_trans, OFy_trans; + +// Lateral Velocity Computation +float Velx, Vely; + +// Compute body velocities +struct FloatVect3 V_Ned; +struct FloatRMat Rmat_Ned2Body; +struct FloatVect3 V_body; + +// Called by plugin +void my_plugin_init(void) +{ + // Initialize variables + gray_frame = (unsigned char *) calloc(imgWidth*imgHeight,sizeof(unsigned char)); + prev_frame = (unsigned char *) calloc(imgWidth*imgHeight*2,sizeof(unsigned char)); + prev_gray_frame = (unsigned char *) calloc(imgWidth*imgHeight,sizeof(unsigned char)); + x = (int *) calloc(MAX_COUNT,sizeof(int)); + new_x = (int *) calloc(MAX_COUNT,sizeof(int)); + y = (int *) calloc(MAX_COUNT,sizeof(int)); + new_y = (int *) calloc(MAX_COUNT,sizeof(int)); + status = (int *) calloc(MAX_COUNT,sizeof(int)); + dx = (int *) calloc(MAX_COUNT,sizeof(int)); + dy = (int *) calloc(MAX_COUNT,sizeof(int)); + old_img_init = 1; + OFx = 0.0; + OFy = 0.0; + dx_sum = 0.0; + dy_sum = 0.0; + diff_roll = 0.0; + diff_pitch = 0.0; + cam_h = 0.0; + prev_pitch = 0.0; + prev_roll = 0.0; + curr_pitch = 0.0; + curr_roll = 0.0; + curr_yaw = 0.0; + OFx_trans = 0.0; + OFy_trans = 0.0; + Velx = 0.0; + Vely = 0.0; +} + +void my_plugin_run(unsigned char *frame) +{ + if(old_img_init == 1) + { + memcpy(prev_frame,frame,imgHeight*imgWidth*2); + CvtYUYV2Gray(prev_gray_frame, prev_frame, imgWidth, imgHeight); + old_img_init = 0; + } + + // *********************************************************************************************************************** + // Additional information from other sensors + // *********************************************************************************************************************** + + // Compute body velocities from ENU + V_Ned.x = stateGetSpeedNed_f()->x; + V_Ned.y = stateGetSpeedNed_f()->y; + V_Ned.z = stateGetSpeedNed_f()->z; + + struct FloatQuat* BodyQuaternions = stateGetNedToBodyQuat_f(); + FLOAT_RMAT_OF_QUAT(Rmat_Ned2Body,*BodyQuaternions); + RMAT_VECT3_MUL(V_body, Rmat_Ned2Body, V_Ned); + + // *********************************************************************************************************************** + // Corner detection + // *********************************************************************************************************************** + + // FAST corner detection + int fast_threshold = 20; + xyFAST* pnts_fast; + pnts_fast = fast9_detect((const byte*)prev_gray_frame, imgWidth, imgHeight, imgWidth, fast_threshold, &count); + + if(count > MAX_COUNT) count = MAX_COUNT; + for(int i = 0; i < count; i++) + { + x[i] = pnts_fast[i].x; + y[i] = pnts_fast[i].y; + } + free(pnts_fast); + + // Remove neighbouring corners + min_distance = 3; + min_distance2 = min_distance*min_distance; + int *labelmin; + labelmin = (int *) calloc(MAX_COUNT,sizeof(int)); + for(int i = 0; i < count; i++) + { + for(int j = i+1; j < count; j++) + { + // distance squared: + 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 = count; + for(int i = count-1; i >= 0; i-- ) + { + remove_point = 0; + + if(labelmin[i]) + { + remove_point = 1; + } + + if(remove_point) + { + for(c = i; c max_count) count_fil = max_count; + count = count_fil; + free(labelmin); + + // ********************************************************************************************************************** + // Corner Tracking + // ********************************************************************************************************************** + CvtYUYV2Gray(gray_frame, frame, imgWidth, imgHeight); + + error_opticflow = opticFlowLK(gray_frame, prev_gray_frame, x, y, count_fil, imgWidth, imgHeight, new_x, new_y, status, 5, 100); + + flow_count = count_fil; + for(int i=count_fil-1; i>=0; i--) + { + remove_point = 1; + + if(status[i] && !(new_x[i] < borderx || new_x[i] > (imgWidth-1-borderx) || + new_y[i] < bordery || new_y[i] > (imgHeight-1-bordery))) + { + remove_point = 0; + } + + if(remove_point) + { + for(c = i; c theta; + curr_roll = stateGetNedToBodyEulers_f()->phi; + curr_yaw = stateGetNedToBodyEulers_f()->psi; + + diff_pitch = (curr_pitch - prev_pitch)*imgHeight/FOV_H; + diff_roll = (curr_roll - prev_roll)*imgWidth/FOV_W; + + prev_pitch = curr_pitch; + prev_roll = curr_roll; + +#ifdef FLOW_DEROTATION + if(flow_count) + { + OFx_trans = dx_sum - diff_roll; + OFy_trans = dy_sum - diff_pitch; + + if((OFx_trans<=0) != (dx_sum<=0)) + { + OFx_trans = 0; + OFy_trans = 0; + } + } + else + { + OFx_trans = dx_sum; + OFy_trans = dy_sum; + } +#else + OFx_trans = dx_sum; + OFy_trans = dy_sum; +#endif + + // Average Filter + OFfilter(&OFx, &OFy, OFx_trans, OFy_trans, flow_count, 1); + + // Velocity Computation + #ifdef USE_SONAR + cam_h = ins_impl.sonar_z; + #else + cam_h = 1; + #endif + + if(flow_count) + { + Velx = OFy*cam_h*FPS/Fy_ARdrone + 0.05; + Vely = -OFx*cam_h*FPS/Fx_ARdrone - 0.1; + } + else + { + Velx = 0.0; + Vely = 0.0; + } + + // ********************************************************************************************************************** + // Next Loop Preparation + // ********************************************************************************************************************** + + memcpy(prev_frame,frame,imgHeight*imgWidth*2); + memcpy(prev_gray_frame,gray_frame,imgHeight*imgWidth); + + // ********************************************************************************************************************** + // Downlink Message + // ********************************************************************************************************************** + DOWNLINK_SEND_OF_HOVER(DefaultChannel, DefaultDevice, &FPS, &dx_sum, &dy_sum, &OFx, &OFy, &diff_roll, &diff_pitch, &Velx, &Vely, &V_body.x, &V_body.y, &cam_h, &count); +} + diff --git a/sw/airborne/modules/computer_vision/OpticFlow/opticflow_code.h b/sw/airborne/modules/computer_vision/OpticFlow/opticflow_code.h new file mode 100644 index 0000000000..f46fcc0d77 --- /dev/null +++ b/sw/airborne/modules/computer_vision/OpticFlow/opticflow_code.h @@ -0,0 +1,49 @@ +/* + * 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, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/* + * @file paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/opticflow_code.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 _OPT_FL_LAND_H +#define _OPT_FL_LAND_H + +// Settable by pluging +extern unsigned int imgWidth, imgHeight; +extern unsigned int verbose; + +// Variables used by the controller +extern float Velx, Vely; +extern int count; +extern int flow_count; +extern struct FloatVect3 V_body; + +// Called by plugin +void my_plugin_init(void); +void my_plugin_run(unsigned char *frame); + +// Timer +void start_timer_rates(void); +long end_timer_rates(void); +#endif diff --git a/sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.c b/sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.c new file mode 100644 index 0000000000..a254d05b1f --- /dev/null +++ b/sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.c @@ -0,0 +1,218 @@ +/* + * 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, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * @file paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/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 + */ + + +// Own header +#include "opticflow_module.h" + +// Navigate Based On Vision +#include "hover_stabilization.h" + +// Paparazzi +#include "state.h" // for attitude +#include "boards/ardrone/navdata.h" // for ultrasound Height + +// Threaded computer vision +#include + +// Frame Rate (FPS) +#include +float FPS; +volatile long timestamp; +#define USEC_PER_SEC 1000000L +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; +} + +struct timeval start_time; +struct timeval end_time; + +void start_timer() { + gettimeofday (&start_time, NULL); +} +long end_timer() { + gettimeofday (&end_time, NULL); + return time_elapsed(&start_time, &end_time); +} + +void opticflow_module_init(void) +{ + // Immediately start the vision thread when the module initialized + opticflow_module_start(); + + // Stabilization Code Initialization + init_hover_stabilization_onvision(); + + // Frame Rate Initialization + FPS = 0.0; + timestamp=0; + start_timer(); +} + + +volatile uint8_t computervision_thread_has_results = 0; + +void opticflow_module_run(void) +{ + + // Read Latest Vision Module Results + if (computervision_thread_has_results) + { + computervision_thread_has_results = 0; + run_hover_stabilization_onvision(); + } +} + +///////////////////////////////////////////////////////////////////////// +// COMPUTER VISION THREAD + +// Video +#include "v4l/video.h" +#include "resize.h" + +// Payload Code +#include "opticflow_code.h" + +// Downlink Video +//#define DOWNLINK_VIDEO 1 + +#ifdef DOWNLINK_VIDEO +#include "encoding/jpeg.h" +#include "encoding/rtp.h" +#endif + +#include + +pthread_t computervision_thread; +volatile uint8_t computervision_thread_status = 0; +volatile uint8_t computer_vision_thread_command = 0; +void *computervision_thread_main(void* data); +void *computervision_thread_main(void* data) +{ + // Video Input + struct vid_struct vid; + vid.device = (char*)"/dev/video2"; // video1 = front camera; video2 = bottom camera + vid.w=320; + vid.h=240; + vid.n_buffers = 4; + + if (video_init(&vid)<0) { + printf("Error initialising video\n"); + computervision_thread_status = -1; + return 0; + } + + // Video Grabbing + struct img_struct* img_new = video_create_image(&vid); + + // Video Resizing + #define DOWNSIZE_FACTOR 1 + struct img_struct small; + small.w = vid.w / DOWNSIZE_FACTOR; + small.h = vid.h / DOWNSIZE_FACTOR; + small.buf = (uint8_t*)malloc(small.w*small.h*2); + +#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 + imgWidth = small.w; + imgHeight = small.h; + verbose = 2; + my_plugin_init(); + + while (computer_vision_thread_command > 0) + { + video_grab_image(&vid, img_new); + + // FPS + timestamp = end_timer(); + FPS = (float) 1000000/(float)timestamp; + // printf("dt = %d, FPS = %f\n",timestamp, FPS); + start_timer(); + + // Resize + //resize_uyuv(img_new, &small, DOWNSIZE_FACTOR); + + // Run Image Processing + my_plugin_run(img_new->buf); + // my_plugin_run(small.buf); + + #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); + uint32_t delta_t_per_frame = 0; // 0 = use drone clock + send_rtp_frame(vsock, jpegbuf,size, small.w, small.h,0, quality_factor, dri_header, delta_t_per_frame); + #endif + computervision_thread_has_results++; + } + printf("Thread Closed\n"); + video_close(&vid); + computervision_thread_status = -100; + return 0; +} + +void opticflow_module_start(void) +{ + computer_vision_thread_command = 1; + int rc = pthread_create(&computervision_thread, NULL, computervision_thread_main, NULL); + if(rc) { + printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc); + } +} + +void opticflow_module_stop(void) +{ + computer_vision_thread_command = 0; +} + + + diff --git a/sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.h b/sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.h new file mode 100644 index 0000000000..b5e635a1ff --- /dev/null +++ b/sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.h @@ -0,0 +1,45 @@ +/* + * 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, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/* + * @file paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/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_LAND_H +#define OPTICFLOW_LAND_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); + +// Frame Rate +extern float FPS; +struct timeval; +long time_elapsed(struct timeval *t1, struct timeval *t2); +void start_timer(void); +long end_timer(void); + +#endif /* OPTICFLOW_LAND_H */ diff --git a/sw/airborne/modules/computer_vision/OpticFlow/video_message_structs.h b/sw/airborne/modules/computer_vision/OpticFlow/video_message_structs.h new file mode 100644 index 0000000000..2039dd4edd --- /dev/null +++ b/sw/airborne/modules/computer_vision/OpticFlow/video_message_structs.h @@ -0,0 +1,32 @@ +#ifndef VMSTRTS_H +#define VMSTRTS_H + +/*** + an exact copy of this file to exist in ppz + this files keeps the structs that are serialized and streamed over tcp/ip through localhost + */ + +#define N_BINS 10 + +struct gst2ppz_message_struct +{ + unsigned int ID; // Keep different modules for using each others data + unsigned int counter; // counter to keep track of data + unsigned int obstacle_bins[N_BINS]; // optical flow output, shift in x direction + unsigned int uncertainty_bins[N_BINS]; //optical flow output, shift in y direction +}; +extern struct gst2ppz_message_struct gst2ppz; + +struct ppz2gst_message_struct +{ + unsigned int ID; // Keep different modules for using each others data + unsigned int counter; //counter to keep track of data + int pitch; + int roll; + int alt; + int adjust_factor; // 0-10 :adjust brightness +}; +extern struct ppz2gst_message_struct ppz2gst; + +#endif /* VMSTRTS_H */ + diff --git a/sw/airborne/modules/computer_vision/cv/color.h b/sw/airborne/modules/computer_vision/cv/color.h index 46ffdb413a..86737831f1 100644 --- a/sw/airborne/modules/computer_vision/cv/color.h +++ b/sw/airborne/modules/computer_vision/cv/color.h @@ -40,10 +40,8 @@ inline void grayscale_uyvy(struct img_struct *input, struct img_struct *output) } } -inline int colorfilt_uyvy(struct img_struct *input, struct img_struct *output, uint8_t y_m, uint8_t y_M, uint8_t u_m, - uint8_t u_M, uint8_t v_m, uint8_t v_M); -inline int colorfilt_uyvy(struct img_struct *input, struct img_struct *output, uint8_t y_m, uint8_t y_M, uint8_t u_m, - uint8_t u_M, uint8_t v_m, uint8_t v_M) +inline int colorfilt_uyvy(struct img_struct *input, struct img_struct *output, uint8_t y_m, uint8_t y_M, uint8_t u_m, uint8_t u_M, uint8_t v_m, uint8_t v_M); +inline int colorfilt_uyvy(struct img_struct *input, struct img_struct *output, uint8_t y_m, uint8_t y_M, uint8_t u_m, uint8_t u_M, uint8_t v_m, uint8_t v_M) { int cnt = 0; uint8_t *source = input->buf; diff --git a/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c b/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c index 119f4bc2cf..7d336dd1d6 100644 --- a/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c +++ b/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c @@ -433,8 +433,7 @@ void MakeTables(int q) -uint8_t *encode_image(uint8_t *input_ptr, uint8_t *output_ptr, uint32_t quality_factor, uint32_t image_format, - uint32_t image_width, uint32_t image_height, uint8_t add_dri_header) +uint8_t *encode_image(uint8_t *input_ptr, uint8_t *output_ptr, uint32_t quality_factor, uint32_t image_format, uint32_t image_width, uint32_t image_height, uint8_t add_dri_header) { uint16_t i, j; diff --git a/sw/airborne/modules/computer_vision/cv/encoding/rtp.c b/sw/airborne/modules/computer_vision/cv/encoding/rtp.c index 2221126818..541da0068e 100644 --- a/sw/airborne/modules/computer_vision/cv/encoding/rtp.c +++ b/sw/airborne/modules/computer_vision/cv/encoding/rtp.c @@ -7,9 +7,7 @@ #include "rtp.h" -void send_rtp_packet(struct UdpSocket *sock, uint8_t *Jpeg, int JpegLen, uint32_t m_SequenceNumber, - uint32_t m_Timestamp, uint32_t m_offset, uint8_t marker_bit, int w, int h, uint8_t format_code, uint8_t quality_code, - uint8_t has_dri_header); +void send_rtp_packet(struct UdpSocket *sock, uint8_t *Jpeg, int JpegLen, uint32_t m_SequenceNumber, uint32_t m_Timestamp, uint32_t m_offset, uint8_t marker_bit, int w, int h, uint8_t format_code, uint8_t quality_code, uint8_t has_dri_header); // http://www.ietf.org/rfc/rfc3550.txt @@ -51,19 +49,16 @@ void test_rtp_frame(struct UdpSocket *sock) uint8_t quality_code = 0x54; if (toggle) { - send_rtp_packet(sock, JpegScanDataCh2A, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code, - quality_code, 0); + send_rtp_packet(sock, JpegScanDataCh2A, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code, quality_code, 0); } else { - send_rtp_packet(sock, JpegScanDataCh2B, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code, - quality_code, 0); + send_rtp_packet(sock, JpegScanDataCh2B, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code, quality_code, 0); } framecounter++; timecounter += 3600; } -void send_rtp_frame(struct UdpSocket *sock, uint8_t *Jpeg, uint32_t JpegLen, int w, int h, uint8_t format_code, - uint8_t quality_code, uint8_t has_dri_header, uint32_t delta_t) +void send_rtp_frame(struct UdpSocket *sock, uint8_t *Jpeg, uint32_t JpegLen, int w, int h, uint8_t format_code, uint8_t quality_code, uint8_t has_dri_header, uint32_t delta_t) { static uint32_t packetcounter = 0; static uint32_t timecounter = 0; @@ -88,8 +83,7 @@ void send_rtp_frame(struct UdpSocket *sock, uint8_t *Jpeg, uint32_t JpegLen, int len = JpegLen; } - send_rtp_packet(sock, Jpeg, len, packetcounter, timecounter, offset, lastpacket, w, h, format_code, quality_code, - has_dri_header); + send_rtp_packet(sock, Jpeg, len, packetcounter, timecounter, offset, lastpacket, w, h, format_code, quality_code, has_dri_header); JpegLen -= len; Jpeg += len; 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..ee48fe6809 --- /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..9601a02e8e --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.c @@ -0,0 +1,6069 @@ +/* +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..88d2571490 --- /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_ardrone.c b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.c new file mode 100644 index 0000000000..f46243b964 --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.c @@ -0,0 +1,624 @@ +/* + * Copyright (C) 2014 Hann Woei Ho + * + * - Initial fixed-point C implementation by G. de Croon + * - Algorithm: Lucas-Kanade by Yves Bouguet + * - Publication: http://robots.stanford.edu/cs223b04/algo_tracking.pdf + * + * 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 paparazzi/sw/ext/ardrone2_vision/cv/opticflow/optic_flow_ardrone.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 +#include +#include +#include +#include "optic_flow_gdc.h" +#include "../../modules/OpticFlow/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; + //int printed, limit; + 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; + //printed = 0; limit = 4; + + 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(printed < limit) + { + printf("x_0_or = %d, y_0_or = %d;\n\r", x_0_or, y_0_or); + printf("x_0 = %d, y_0 = %d\n\r"); + printed++; + }*/ + + + if (x == x_0 && y == y_0) { + // simply copy the pixel: +// ix2 = uint_index(x_0_or, y_0_or); +// Y = ((unsigned int)frame_buf[ix2+1] + (unsigned int)frame_buf[ix2+3]) >> 1; + 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]; +// ix2 = uint_index(x_0_or, y_0_or); +// Y = ((unsigned int)frame_buf[ix2+1] + (unsigned int)frame_buf[ix2+3]) >> 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]; +// ix2 = uint_index((x_0_or+1), y_0_or); +// Y = ((unsigned int)frame_buf[ix2+1] + (unsigned int)frame_buf[ix2+3]) >> 1; + //if(printed < limit) printf("subpixel: TR = %d\n\r", Y); + 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]; +// ix2 = uint_index(x_0_or, (y_0_or+1)); +// Y = ((unsigned int)frame_buf[ix2+1] + (unsigned int)frame_buf[ix2+3]) >> 1; + //if(printed < limit) printf("subpixel: BL = %d\n\r", Y); + 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]; +// ix2 = uint_index((x_0_or+1), (y_0_or+1)); +// Y = ((unsigned int)frame_buf[ix2+1] + (unsigned int)frame_buf[ix2+3]) >> 1; + //if(printed < limit) printf("subpixel: BR = %d\n\r", Y); + Patch[ix1] += alpha_x * alpha_y * ((int) Y); + + // normalize patch value + Patch[ix1] /= (subpixel_factor * subpixel_factor); + + /*if(printed < limit) + { + + printf("alpha_x = %d, alpha_y = %d, x_0 = %d, y_0 = %d, x = %d, y = %d, Patch[ix1] = %d\n\r", alpha_x, alpha_y, x_0, y_0, x, y, Patch[ix1]); + // printed++; + } + */ + + } + } + } + + 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; + 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; + DY[ix2] = (Y2 - Y1) / 2; + + /*if(printed < 1 && DX[ix2] > 0) + { + printf("DX = %d, DY = %d\n\r", DX[ix2], DY[ix2]); + printed++; + } + else if(printed == 1 && DX[ix2] < 0) + { + printf("DX = %d, DY = %d\n\r", DX[ix2], DY[ix2]); + printed++; + }*/ + + + } + } + + 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; + //if(sum < threshold && sum > -threshold) + //{ + sum += Patch[ix]; // do not check thresholds + //} + /*else + { + if(sum > threshold) + { + sum = threshold; + } + else + { + sum = -threshold; + } + }*/ + } + } + + 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++) { + //printf("*** NEW POINT ***\n\r"); + // status: point is not yet lost: + status[p] = 1; + + //printf("Normal coordinate: (%d,%d)\n\r", p_x[p], p_y[p]); + // 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; + //printf("Subpixel coordinate: (%d,%d)\n\r", p_x[p], p_y[p]); + + // 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)) { +// printf("Outside of ROI, P1[%d,%d]\n\r",p_x[p],p_y[p]); + 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++) { + // printf("G[%d] = %d\n\r", it, G[it]); + G[it] /= 255; // to keep values in range + // printf("G[%d] = %d\n\r", it, G[it]); + } + // calculate G's determinant: + Det = G[0] * G[3] - G[1] * G[2]; + //printf("Det = %d\n\r", Det); + Det = Det / subpixel_factor; // so that the steps will be expressed in subpixel units + //printf("Det = %d\n\r", Det); + if (Det < 1) { + status[p] = 0; +// printf("irrevertible G\n"); + } + + // (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)) { + //printf("it = %d, (p_x+v_x,p_y+v_y) = (%d,%d)\n\r", it, p_x[p]+v_x, p_y[p]+v_y); + //printf("it = %d;", it); + // 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)) { +// printf("Outside of ROI, P1[%d,%d]\n\r",p_x[p],p_y[p]); + 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 + //printf("I = "); + //printIntMatrix(I_neighborhood, patch_size, patch_size); + //printf("J = "); + //printIntMatrix(J_neighborhood, patch_size, patch_size); + //getSubPixel(J_neighborhood, new_image_buf, subpixel_factor * ((p_x[p]+v_x)/subpixel_factor), subpixel_factor * ((p_y[p]+v_y) / subpixel_factor), half_window_size, subpixel_factor); + //printf("J2 = "); + //printIntMatrix(J_neighborhood, patch_size, patch_size); + //printf("figure(); subplot(1,2,1); imshow(I/255); subplot(1,2,2); imshow(J/255);\n\r"); + getImageDifference(I_neighborhood, J_neighborhood, ImDiff, patch_size, patch_size); + //printf("ImDiff = "); + //printIntMatrix(ImDiff, patch_size, patch_size); + error = calculateError(ImDiff, patch_size, patch_size) / 255; + +// if(error > error_threshold) printf("error threshold\n"); + if (error > error_threshold && it > max_iterations / 2) { + status[p] = 0; +// printf("occlusion\n"); + break; + } + //printf("error(%d) = %d;\n\r", it+1, error); + // [c] calculate the 'b'-vector + //printf("DX = "); + //printIntMatrix(DX, patch_size, patch_size); + multiplyImages(ImDiff, DX, IDDX, patch_size, patch_size); + //printf("IDDX = "); + //printIntMatrix(IDDX, patch_size, patch_size); + multiplyImages(ImDiff, DY, IDDY, patch_size, patch_size); + //printf("DY = "); + //printIntMatrix(DY, patch_size, patch_size); + //printf("IDDY = "); + //printIntMatrix(IDDY, patch_size, patch_size); + //printf("figure(); subplot(2,3,1); imagesc(ImDiff); subplot(2,3,2); imagesc(DX); subplot(2,3,3); imagesc(DY);"); + //printf("subplot(2,3,4); imagesc(IDDY); subplot(2,3,5); imagesc(IDDX);\n\r"); + // division by 255 to keep values in range: + 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 + //printf("step = [%d,%d]; v = [%d,%d];\n\r", step_x, step_y, v_x, v_y); + //printf("pause(0.5);\n\r"); + // next iteration + it++; + // step_size = abs(step_x); + // step_size += abs(step_y); + //printf("status = %d, it = %d, step_size = %d\n\r", status[p], it, step_size); + } // iteration to find the right window in the new image + + //printf("figure(); plot(error(1:(it+1)));\n\r"); +// printf("it = %d\n",it); + 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_ardrone.h b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.h new file mode 100644 index 0000000000..14cc568efe --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.h @@ -0,0 +1,43 @@ +/* + * 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, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/* + * @file paparazzi/sw/ext/ardrone2_vision/cv/opticflow/optic_flow_gdc.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 OPTIC +#define OPTIC +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 diff --git a/sw/airborne/modules/computer_vision/cv/resize.h b/sw/airborne/modules/computer_vision/cv/resize.h index e2bbf7096f..22f06888cf 100644 --- a/sw/airborne/modules/computer_vision/cv/resize.h +++ b/sw/airborne/modules/computer_vision/cv/resize.h @@ -36,13 +36,13 @@ inline void resize_uyuv(struct img_struct *input, struct img_struct *output, int *dest++ = *source++; // U *dest++ = *source++; // Y // now skip 3 pixels - if (pixelskip) { source += (pixelskip + 1) * 2; } + source += (pixelskip + 1) * 2; *dest++ = *source++; // U *dest++ = *source++; // V - if (pixelskip) { source += (pixelskip - 1) * 2; } + source += (pixelskip - 1) * 2; } // skip 3 rows - if (pixelskip) { source += pixelskip * input->w * 2; } + source += pixelskip * 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..47110a216f --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv/trig.c @@ -0,0 +1,158 @@ +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * 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/paparazzi.h b/sw/airborne/modules/computer_vision/lib/paparazzi.h new file mode 100644 index 0000000000..f3b2aef8f1 --- /dev/null +++ b/sw/airborne/modules/computer_vision/lib/paparazzi.h @@ -0,0 +1,32 @@ + +//////////////////////////////////////////////// +// Paparazzi communication interface + +#include // pthread_create +#include // gprint + +#include "socket.h" +#include "video_message_structs.h" + +struct gst2ppz_message_struct gst2ppz; +struct ppz2gst_message_struct ppz2gst; + +inline void paparazzi_message_server_start(void); +inline void paparazzi_message_send(void); + +struct UdpSocket *sock; + +inline void paparazzi_message_server_start(void) +{ + sock = udp_socket("192.168.1.1", 2000, 2001, FMS_UNICAST); +} + + +inline void paparazzi_message_send(void) +{ + udp_write(sock, (char *) &gst2ppz, sizeof(gst2ppz)); + int ret = udp_read(sock, (unsigned char *) &ppz2gst, sizeof(ppz2gst)); + printf("read: %d \n",ret); +} + + diff --git a/sw/airborne/modules/computer_vision/lib/tcp/socket.c b/sw/airborne/modules/computer_vision/lib/tcp/socket.c new file mode 100644 index 0000000000..9479c698fa --- /dev/null +++ b/sw/airborne/modules/computer_vision/lib/tcp/socket.c @@ -0,0 +1,139 @@ +#include "socket.h" +#include /* socket definitions */ +#include /* socket types */ +#include /* inet (3) funtions */ +#include /* misc. UNIX functions */ +#include +#include /* memset */ + +#include +#include + + + + +/* Global constants */ + +#define ECHO_PORT (2002) +#define MAX_LINE (1000) + +/* Global variables */ +int list_s; /* listening socket */ +int conn_s; /* connection socket */ +struct sockaddr_in servaddr; /* socket address structure */ +char buffer[MAX_LINE]; /* character buffer */ +char *endptr; /* for strtol() */ + +int closeSocket(void) +{ + return close(conn_s); +} + +int initSocket(void) +{ + + /* Create the listening socket */ + + if ((list_s = socket(AF_INET, SOCK_STREAM, 0)) < 0) { + fprintf(stderr, "ECHOSERV: Error creating listening socket.\n"); + return -1; + } + + + /* Set all bytes in socket address structure to + zero, and fill in the relevant data members */ + + memset(&servaddr, 0, sizeof(servaddr)); + servaddr.sin_family = AF_INET; + servaddr.sin_addr.s_addr = htonl(INADDR_ANY); + servaddr.sin_port = htons(ECHO_PORT); + + /* Bind our socket addresss to the + listening socket, and call listen() */ + + if (bind(list_s, (struct sockaddr *) &servaddr, sizeof(servaddr)) < 0) { + fprintf(stderr, "ECHOSERV: Error calling bind()\n"); + exit(EXIT_FAILURE); + } + + if (listen(list_s, LISTENQ) < 0) { + fprintf(stderr, "ECHOSERV: Error calling listen()\n"); + exit(EXIT_FAILURE); + } + + + + /* Wait for a connection, then accept() it */ + if ((conn_s = accept(list_s, NULL, NULL)) < 0) { + fprintf(stderr, "ECHOSERV: Error calling function accept()\n"); + return -1; + } + printf("Connected!\n"); + return 1; +} + +/* Read a line from a socket */ + +ssize_t Readline_socket(void *vptr, size_t maxlen) +{ + ssize_t n, rc; + char c, *buffer; + + buffer = vptr; + + for (n = 1; n < maxlen; n++) { + + if ((rc = read(conn_s, &c, 1)) == 1) { + *buffer++ = c; + if (c == '\n') { + break; + } + } else if (rc == 0) { + if (n == 1) { + return 0; + } else { + break; + } + } else { + if (errno == EINTR) { + continue; + } + return -1; + } + } + + *buffer = 0; + return n; +} + + +/* Write a line to a socket */ + +ssize_t Writeline_socket(struct img_struct *img, size_t n) +{ + size_t nleft; + ssize_t nwritten; + unsigned char *buf1 = img->buf; + nleft = n; + + //printf("socket buf1 %d, %d, %d, %d, %d, %d\n",buf1[0],buf1[1],buf1[2],buf1[3],buf1[4],buf1[5]); + + while (nleft > 0) { + if ((nwritten = write(conn_s, buf1, nleft)) <= 0) { + if (errno == EINTR) { + nwritten = 0; + } else { + return -1; + } + } + nleft -= nwritten; + buf1 += nwritten; + } + + return n; + +} + + + + diff --git a/sw/airborne/modules/computer_vision/lib/tcp/socket.h b/sw/airborne/modules/computer_vision/lib/tcp/socket.h new file mode 100644 index 0000000000..04091cd484 --- /dev/null +++ b/sw/airborne/modules/computer_vision/lib/tcp/socket.h @@ -0,0 +1,17 @@ +#ifndef SOCKET_H +#define SOCKET_H + +#include /* for ssize_t data type */ +#include "./video/video.h" + +#define LISTENQ (1024) /* Backlog for listen() */ + + +/* Function declarations */ +int initSocket(void) ; +ssize_t Readline(void *vptr, size_t maxlen); +ssize_t Writeline(struct img_struct *img, size_t maxlen); +int closeSocket(void); + +#endif /* SOCKET_H */ + diff --git a/sw/airborne/modules/computer_vision/lib/udp/socket.c b/sw/airborne/modules/computer_vision/lib/udp/socket.c index fba489e282..34d73e2023 100644 --- a/sw/airborne/modules/computer_vision/lib/udp/socket.c +++ b/sw/airborne/modules/computer_vision/lib/udp/socket.c @@ -22,8 +22,7 @@ # define SOCKET_ERROR -1 # define IO_SOCKET ioctl -struct UdpSocket *udp_socket(const char *str_ip_out, const int port_out, const int port_in, const int broadcast) -{ +struct UdpSocket *udp_socket(const char *str_ip_out, const int port_out, const int port_in, const int broadcast) { struct UdpSocket *me = malloc(sizeof(struct UdpSocket)); diff --git a/sw/airborne/modules/computer_vision/lib/v4l/ardrone2.c b/sw/airborne/modules/computer_vision/lib/v4l/ardrone2.c new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sw/airborne/modules/computer_vision/lib/v4l/ardrone2.h b/sw/airborne/modules/computer_vision/lib/v4l/ardrone2.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sw/airborne/modules/computer_vision/lib/v4l/video.c b/sw/airborne/modules/computer_vision/lib/v4l/video.c index e0a9fb5c31..b3a090ced4 100644 --- a/sw/airborne/modules/computer_vision/lib/v4l/video.c +++ b/sw/airborne/modules/computer_vision/lib/v4l/video.c @@ -43,10 +43,10 @@ pthread_t video_thread; -void *video_thread_main(void *data); -void *video_thread_main(void *data) +void *video_thread_main(void* data); +void *video_thread_main(void* data) { - struct vid_struct *vid = (struct vid_struct *)data; + struct vid_struct* vid = (struct vid_struct*)data; printf("video_thread_main started\n"); while (1) { fd_set fds; @@ -61,7 +61,7 @@ void *video_thread_main(void *data) r = select(vid->fd + 1, &fds, NULL, NULL, &tv); if (-1 == r) { - if (EINTR == errno) { continue; } + if (EINTR == errno) continue; printf("select err\n"); } @@ -84,13 +84,13 @@ void *video_thread_main(void *data) vid->seq++; - if (vid->trigger) { + if(vid->trigger) { // todo add timestamp again //vid->img->timestamp = util_timestamp(); vid->img->seq = vid->seq; - memcpy(vid->img->buf, vid->buffers[buf.index].buf, vid->w * vid->h * 2); - vid->trigger = 0; + memcpy(vid->img->buf, vid->buffers[buf.index].buf, vid->w*vid->h*2); + vid->trigger=0; } if (ioctl(vid->fd, VIDIOC_QBUF, &buf) < 0) { @@ -108,9 +108,9 @@ int video_init(struct vid_struct *vid) unsigned int i; enum v4l2_buf_type type; - vid->seq = 0; - vid->trigger = 0; - if (vid->n_buffers == 0) { vid->n_buffers = 4; } + vid->seq=0; + vid->trigger=0; + if(vid->n_buffers==0) vid->n_buffers=4; vid->fd = open(vid->device, O_RDWR | O_NONBLOCK, 0); @@ -148,7 +148,7 @@ int video_init(struct vid_struct *vid) printf("Buffer count = %d\n", vid->n_buffers); - vid->buffers = (struct buffer_struct *)calloc(vid->n_buffers, sizeof(struct buffer_struct)); + vid->buffers = (struct buffer_struct*)calloc(vid->n_buffers, sizeof(struct buffer_struct)); for (i = 0; i < vid->n_buffers; ++i) { struct v4l2_buffer buf; @@ -164,11 +164,11 @@ int video_init(struct vid_struct *vid) } vid->buffers[i].length = buf.length; - printf("buffer%d.length=%d\n", i, buf.length); - vid->buffers[i].buf = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, vid->fd, buf.m.offset); + printf("buffer%d.length=%d\n",i,buf.length); + vid->buffers[i].buf = mmap(NULL, buf.length, PROT_READ|PROT_WRITE, MAP_SHARED, vid->fd, buf.m.offset); if (MAP_FAILED == vid->buffers[i].buf) { - printf("mmap() failed.\n"); + printf ("mmap() failed.\n"); return -1; } } @@ -188,14 +188,14 @@ int video_init(struct vid_struct *vid) } type = V4L2_BUF_TYPE_VIDEO_CAPTURE; - if (ioctl(vid->fd, VIDIOC_STREAMON, &type) < 0) { + if (ioctl(vid->fd, VIDIOC_STREAMON, &type)< 0) { printf("ioctl() VIDIOC_STREAMON failed.\n"); return -1; } //start video thread int rc = pthread_create(&video_thread, NULL, video_thread_main, vid); - if (rc) { + if(rc) { printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc); return 202; } @@ -207,28 +207,27 @@ void video_close(struct vid_struct *vid) { int i; for (i = 0; i < (int)vid->n_buffers; ++i) { - if (-1 == munmap(vid->buffers[i].buf, vid->buffers[i].length)) { printf("munmap() failed.\n"); } + if (-1 == munmap(vid->buffers[i].buf, vid->buffers[i].length)) printf("munmap() failed.\n"); } close(vid->fd); } struct img_struct *video_create_image(struct vid_struct *vid) { - struct img_struct *img = (struct img_struct *)malloc(sizeof(struct img_struct)); - img->w = vid->w; - img->h = vid->h; - img->buf = (unsigned char *)malloc(vid->h * vid->w * 2); + struct img_struct* img = (struct img_struct*)malloc(sizeof(struct img_struct)); + img->w=vid->w; + img->h=vid->h; + img->buf = (unsigned char*)malloc(vid->h*vid->w*2); return img; } pthread_mutex_t video_grab_mutex = PTHREAD_MUTEX_INITIALIZER; -void video_grab_image(struct vid_struct *vid, struct img_struct *img) -{ +void video_grab_image(struct vid_struct *vid, struct img_struct *img) { pthread_mutex_lock(&video_grab_mutex); vid->img = img; - vid->trigger = 1; + vid->trigger=1; // while(vid->trigger) pthread_yield(); - while (vid->trigger) { usleep(1); } + while(vid->trigger) usleep(1); pthread_mutex_unlock(&video_grab_mutex); } diff --git a/sw/airborne/modules/computer_vision/lib/v4l/video.h b/sw/airborne/modules/computer_vision/lib/v4l/video.h index 351ba68522..125892ad1b 100644 --- a/sw/airborne/modules/computer_vision/lib/v4l/video.h +++ b/sw/airborne/modules/computer_vision/lib/v4l/video.h @@ -25,7 +25,7 @@ #include "../../cv/image.h" struct buffer_struct { - void *buf; + void * buf; size_t length; }; @@ -39,7 +39,7 @@ struct vid_struct { //private members int trigger; struct img_struct *img; - struct buffer_struct *buffers; + struct buffer_struct * buffers; int fd; }; From 4a8105fc847f6f6c06cdc9d182b04726c0c5ecf0 Mon Sep 17 00:00:00 2001 From: dewagter Date: Mon, 12 Jan 2015 20:47:19 +0100 Subject: [PATCH 02/99] Re-organize in new dir structure --- conf/modules/cv_opticflow.xml | 51 +++++++++++++++++++ .../OpticFlow/OpticFlowHover.xml | 44 ---------------- .../{OpticFlow => opticflow}/dummy.c | 0 .../hover_stabilization.c | 0 .../hover_stabilization.h | 0 .../{OpticFlow => opticflow}/opticflow_code.c | 6 +-- .../{OpticFlow => opticflow}/opticflow_code.h | 0 .../video_message_structs.h | 0 .../{OpticFlow => }/opticflow_module.c | 4 +- .../{OpticFlow => }/opticflow_module.h | 0 10 files changed, 56 insertions(+), 49 deletions(-) create mode 100644 conf/modules/cv_opticflow.xml delete mode 100644 sw/airborne/modules/computer_vision/OpticFlow/OpticFlowHover.xml rename sw/airborne/modules/computer_vision/{OpticFlow => opticflow}/dummy.c (100%) rename sw/airborne/modules/computer_vision/{OpticFlow => opticflow}/hover_stabilization.c (100%) rename sw/airborne/modules/computer_vision/{OpticFlow => opticflow}/hover_stabilization.h (100%) rename sw/airborne/modules/computer_vision/{OpticFlow => opticflow}/opticflow_code.c (98%) rename sw/airborne/modules/computer_vision/{OpticFlow => opticflow}/opticflow_code.h (100%) rename sw/airborne/modules/computer_vision/{OpticFlow => opticflow}/video_message_structs.h (100%) rename sw/airborne/modules/computer_vision/{OpticFlow => }/opticflow_module.c (98%) rename sw/airborne/modules/computer_vision/{OpticFlow => }/opticflow_module.h (100%) diff --git a/conf/modules/cv_opticflow.xml b/conf/modules/cv_opticflow.xml new file mode 100644 index 0000000000..5493c8db3c --- /dev/null +++ b/conf/modules/cv_opticflow.xml @@ -0,0 +1,51 @@ + + + + + Video ARDone 2 + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sw/airborne/modules/computer_vision/OpticFlow/OpticFlowHover.xml b/sw/airborne/modules/computer_vision/OpticFlow/OpticFlowHover.xml deleted file mode 100644 index 3614379eb9..0000000000 --- a/sw/airborne/modules/computer_vision/OpticFlow/OpticFlowHover.xml +++ /dev/null @@ -1,44 +0,0 @@ - - - - - Video ARDone 2 - - -
- -
- - - - - - - - - -include $(PAPARAZZI_HOME)/sw/ext/ardrone2_vision/Makefile.paths - -VISION_MODULE_FOLDER = $(DIR_MODULES)/OpticFlow - -$(TARGET).CFLAGS += -I$(DIR_MODULES) -I$(DIR_CV) -I$(DIR_LIB) -pthread -D__USE_GNU -$(TARGET).CXXFLAGS += -I$(PAPARAZZI_HOME)/sw/include/ -I$(PAPARAZZI_SRC)/sw/airborne -I$(PAPARAZZI_SRC)/conf/autopilot -I$(PAPARAZZI_SRC)/sw/airborne/arch/$($(TARGET).ARCHDIR) -I$(VARINCLUDE) -I$(ACINCLUDE) -I$(PAPARAZZI_SRC)/sw/airborne/modules/ -$(TARGET).CXXFLAGS += -I$(DIR_MODULES) -I$(DIR_CV) -I$(DIR_LIB) -pthread -D__USE_GNU - -$(TARGET).srcs += $(VISION_MODULE_FOLDER)/opticflow_module.c -$(TARGET).srcs += $(VISION_MODULE_FOLDER)/opticflow_code.c -$(TARGET).srcs += $(VISION_MODULE_FOLDER)/hover_stabilization.c -$(TARGET).srcs += $(DIR_CV)/opticflow/optic_flow_ardrone.c -$(TARGET).srcs += $(DIR_CV)/opticflow/fast9/fastRosten.c -$(TARGET).srcs += $(DIR_CV)/encoding/jpeg.c -$(TARGET).srcs += $(DIR_CV)/encoding/rtp.c -$(TARGET).srcs += $(DIR_CV)/trig.c -$(TARGET).srcs += $(DIR_LIB)/udp/socket.c -$(TARGET).srcs += $(DIR_LIB)/v4l/video.c -$(TARGET).CFLAGS += -I$(DIR_MODULES) -I$(DIR_CV) -I$(DIR_LIB) -pthread -$(TARGET).LDFLAGS += -pthread -lrt -static - - - - - diff --git a/sw/airborne/modules/computer_vision/OpticFlow/dummy.c b/sw/airborne/modules/computer_vision/opticflow/dummy.c similarity index 100% rename from sw/airborne/modules/computer_vision/OpticFlow/dummy.c rename to sw/airborne/modules/computer_vision/opticflow/dummy.c diff --git a/sw/airborne/modules/computer_vision/OpticFlow/hover_stabilization.c b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c similarity index 100% rename from sw/airborne/modules/computer_vision/OpticFlow/hover_stabilization.c rename to sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c diff --git a/sw/airborne/modules/computer_vision/OpticFlow/hover_stabilization.h b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.h similarity index 100% rename from sw/airborne/modules/computer_vision/OpticFlow/hover_stabilization.h rename to sw/airborne/modules/computer_vision/opticflow/hover_stabilization.h diff --git a/sw/airborne/modules/computer_vision/OpticFlow/opticflow_code.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_code.c similarity index 98% rename from sw/airborne/modules/computer_vision/OpticFlow/opticflow_code.c rename to sw/airborne/modules/computer_vision/opticflow/opticflow_code.c index bac9bfc338..ee624f2a43 100644 --- a/sw/airborne/modules/computer_vision/OpticFlow/opticflow_code.c +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_code.c @@ -33,9 +33,9 @@ #include "opticflow_code.h" // Computer Vision -#include "opticflow/optic_flow_gdc.h" +#include "opticflow/optic_flow_ardrone.h" #include "opticflow/fast9/fastRosten.h" -#include "opticflow_module.h" +#include "../opticflow_module.h" // Paparazzi Data #include "subsystems/ins/ins_int.h" @@ -301,7 +301,7 @@ void my_plugin_run(unsigned char *frame) // Velocity Computation #ifdef USE_SONAR - cam_h = ins_impl.sonar_z; + cam_h = 1; //ins_impl.sonar_z; #else cam_h = 1; #endif diff --git a/sw/airborne/modules/computer_vision/OpticFlow/opticflow_code.h b/sw/airborne/modules/computer_vision/opticflow/opticflow_code.h similarity index 100% rename from sw/airborne/modules/computer_vision/OpticFlow/opticflow_code.h rename to sw/airborne/modules/computer_vision/opticflow/opticflow_code.h diff --git a/sw/airborne/modules/computer_vision/OpticFlow/video_message_structs.h b/sw/airborne/modules/computer_vision/opticflow/video_message_structs.h similarity index 100% rename from sw/airborne/modules/computer_vision/OpticFlow/video_message_structs.h rename to sw/airborne/modules/computer_vision/opticflow/video_message_structs.h diff --git a/sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c similarity index 98% rename from sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.c rename to sw/airborne/modules/computer_vision/opticflow_module.c index a254d05b1f..845497f896 100644 --- a/sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -31,7 +31,7 @@ #include "opticflow_module.h" // Navigate Based On Vision -#include "hover_stabilization.h" +#include "opticflow/hover_stabilization.h" // Paparazzi #include "state.h" // for attitude @@ -105,7 +105,7 @@ void opticflow_module_run(void) #include "resize.h" // Payload Code -#include "opticflow_code.h" +#include "opticflow/opticflow_code.h" // Downlink Video //#define DOWNLINK_VIDEO 1 diff --git a/sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.h b/sw/airborne/modules/computer_vision/opticflow_module.h similarity index 100% rename from sw/airborne/modules/computer_vision/OpticFlow/opticflow_module.h rename to sw/airborne/modules/computer_vision/opticflow_module.h From e2249dd50b83271f7480f93fa83a275292b9df14 Mon Sep 17 00:00:00 2001 From: dewagter Date: Mon, 12 Jan 2015 22:03:39 +0100 Subject: [PATCH 03/99] Code style (new) --- .../modules/computer_vision/cv/color.h | 6 +- .../computer_vision/cv/encoding/jpeg.c | 3 +- .../modules/computer_vision/cv/encoding/rtp.c | 16 +- .../cv/opticflow/fast9/LICENSE | 28 +- .../cv/opticflow/fast9/fastRosten.c | 12995 +++++++++------- .../cv/opticflow/fast9/fastRosten.h | 28 +- .../cv/opticflow/optic_flow_ardrone.c | 14 +- .../cv/opticflow/optic_flow_ardrone.h | 6 +- sw/airborne/modules/computer_vision/cv/trig.c | 3 +- .../modules/computer_vision/lib/paparazzi.h | 2 +- .../modules/computer_vision/lib/udp/socket.c | 3 +- .../modules/computer_vision/lib/v4l/video.c | 49 +- .../modules/computer_vision/lib/v4l/video.h | 4 +- .../modules/computer_vision/opticflow/dummy.c | 5 +- .../opticflow/hover_stabilization.c | 137 +- .../opticflow/opticflow_code.c | 368 +- .../opticflow/video_message_structs.h | 14 +- .../computer_vision/opticflow_module.c | 187 +- 18 files changed, 7549 insertions(+), 6319 deletions(-) diff --git a/sw/airborne/modules/computer_vision/cv/color.h b/sw/airborne/modules/computer_vision/cv/color.h index 86737831f1..46ffdb413a 100644 --- a/sw/airborne/modules/computer_vision/cv/color.h +++ b/sw/airborne/modules/computer_vision/cv/color.h @@ -40,8 +40,10 @@ inline void grayscale_uyvy(struct img_struct *input, struct img_struct *output) } } -inline int colorfilt_uyvy(struct img_struct *input, struct img_struct *output, uint8_t y_m, uint8_t y_M, uint8_t u_m, uint8_t u_M, uint8_t v_m, uint8_t v_M); -inline int colorfilt_uyvy(struct img_struct *input, struct img_struct *output, uint8_t y_m, uint8_t y_M, uint8_t u_m, uint8_t u_M, uint8_t v_m, uint8_t v_M) +inline int colorfilt_uyvy(struct img_struct *input, struct img_struct *output, uint8_t y_m, uint8_t y_M, uint8_t u_m, + uint8_t u_M, uint8_t v_m, uint8_t v_M); +inline int colorfilt_uyvy(struct img_struct *input, struct img_struct *output, uint8_t y_m, uint8_t y_M, uint8_t u_m, + uint8_t u_M, uint8_t v_m, uint8_t v_M) { int cnt = 0; uint8_t *source = input->buf; diff --git a/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c b/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c index 7d336dd1d6..119f4bc2cf 100644 --- a/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c +++ b/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c @@ -433,7 +433,8 @@ void MakeTables(int q) -uint8_t *encode_image(uint8_t *input_ptr, uint8_t *output_ptr, uint32_t quality_factor, uint32_t image_format, uint32_t image_width, uint32_t image_height, uint8_t add_dri_header) +uint8_t *encode_image(uint8_t *input_ptr, uint8_t *output_ptr, uint32_t quality_factor, uint32_t image_format, + uint32_t image_width, uint32_t image_height, uint8_t add_dri_header) { uint16_t i, j; diff --git a/sw/airborne/modules/computer_vision/cv/encoding/rtp.c b/sw/airborne/modules/computer_vision/cv/encoding/rtp.c index 541da0068e..2221126818 100644 --- a/sw/airborne/modules/computer_vision/cv/encoding/rtp.c +++ b/sw/airborne/modules/computer_vision/cv/encoding/rtp.c @@ -7,7 +7,9 @@ #include "rtp.h" -void send_rtp_packet(struct UdpSocket *sock, uint8_t *Jpeg, int JpegLen, uint32_t m_SequenceNumber, uint32_t m_Timestamp, uint32_t m_offset, uint8_t marker_bit, int w, int h, uint8_t format_code, uint8_t quality_code, uint8_t has_dri_header); +void send_rtp_packet(struct UdpSocket *sock, uint8_t *Jpeg, int JpegLen, uint32_t m_SequenceNumber, + uint32_t m_Timestamp, uint32_t m_offset, uint8_t marker_bit, int w, int h, uint8_t format_code, uint8_t quality_code, + uint8_t has_dri_header); // http://www.ietf.org/rfc/rfc3550.txt @@ -49,16 +51,19 @@ void test_rtp_frame(struct UdpSocket *sock) uint8_t quality_code = 0x54; if (toggle) { - send_rtp_packet(sock, JpegScanDataCh2A, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code, quality_code, 0); + send_rtp_packet(sock, JpegScanDataCh2A, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code, + quality_code, 0); } else { - send_rtp_packet(sock, JpegScanDataCh2B, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code, quality_code, 0); + send_rtp_packet(sock, JpegScanDataCh2B, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code, + quality_code, 0); } framecounter++; timecounter += 3600; } -void send_rtp_frame(struct UdpSocket *sock, uint8_t *Jpeg, uint32_t JpegLen, int w, int h, uint8_t format_code, uint8_t quality_code, uint8_t has_dri_header, uint32_t delta_t) +void send_rtp_frame(struct UdpSocket *sock, uint8_t *Jpeg, uint32_t JpegLen, int w, int h, uint8_t format_code, + uint8_t quality_code, uint8_t has_dri_header, uint32_t delta_t) { static uint32_t packetcounter = 0; static uint32_t timecounter = 0; @@ -83,7 +88,8 @@ void send_rtp_frame(struct UdpSocket *sock, uint8_t *Jpeg, uint32_t JpegLen, int len = JpegLen; } - send_rtp_packet(sock, Jpeg, len, packetcounter, timecounter, offset, lastpacket, w, h, format_code, quality_code, has_dri_header); + send_rtp_packet(sock, Jpeg, len, packetcounter, timecounter, offset, lastpacket, w, h, format_code, quality_code, + has_dri_header); JpegLen -= len; Jpeg += len; diff --git a/sw/airborne/modules/computer_vision/cv/opticflow/fast9/LICENSE b/sw/airborne/modules/computer_vision/cv/opticflow/fast9/LICENSE index ee48fe6809..63a85126e5 100644 --- a/sw/airborne/modules/computer_vision/cv/opticflow/fast9/LICENSE +++ b/sw/airborne/modules/computer_vision/cv/opticflow/fast9/LICENSE @@ -1,4 +1,4 @@ -Copyright (c) 2006, 2008 Edward Rosten +Copyright(c) 2006, 2008 Edward Rosten All rights reserved. Redistribution and use in source and binary forms, with or without @@ -6,25 +6,25 @@ 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 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. +*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. +*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 +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 index 9601a02e8e..a8d856f9b3 100644 --- a/sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.c +++ b/sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.c @@ -7,16 +7,16 @@ 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 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. + *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. + *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 @@ -36,6034 +36,7285 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #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 *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; + 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); + 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); + free(corners); + free(scores); - return nonmax; + return nonmax; } -xyFAST* nonmax_suppression(const xyFAST* corners, const int* scores, int num_corners, int* ret_num_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; + 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 + /*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; + 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)); + if (num_corners < 1) { + *ret_num_nonmax = 0; + return 0; + } - /* 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)); + ret_nonmax = (xyFAST *)malloc(num_corners * sizeof(xyFAST)); - 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++) - {} + /* 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(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: - ; - } + for (i = 0; i < last_row + 1; i++) { + row_start[i] = -1; + } - free(row_start); - *ret_num_nonmax = num_nonmax; - return ret_nonmax; + { + 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; +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) + 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 - goto is_not_a_corner; - else - goto is_not_a_corner; - else - goto is_not_a_corner; - else - goto is_not_a_corner; - else - 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) + } else if (p[pixel[15]] > cb) { 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 + } else { goto is_not_a_corner; - else - goto is_not_a_corner; - else - goto is_not_a_corner; - else - 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) + } + 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 + } else { goto is_not_a_corner; - else - goto is_not_a_corner; - else + } + else { goto is_not_a_corner; - else - goto is_not_a_corner; - else - 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 + } + 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 - 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 + } + 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 + } + else { goto is_not_a_corner; - else - 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) + } + else if (p[pixel[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) { 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 + } else { goto is_not_a_corner; - else + } + else { goto is_not_a_corner; - else - goto is_not_a_corner; - else + } + 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 - goto is_not_a_corner; - else + } + 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 - goto is_not_a_corner; - else + } + 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 + } + 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 - 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 + } + 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 + } + else { goto is_not_a_corner; - else - goto is_not_a_corner; - else + } + 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 + } + else { 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 + } + 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 + } + 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 + } + 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 + } + else { goto is_not_a_corner; - else - goto is_not_a_corner; - else + } + 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 + } + 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 - 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 + } + 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 + } + 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 + } + else { goto is_not_a_corner; - else + } + 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 + } + 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 - 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 + } + 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 + } + else { + goto is_not_a_corner; + } + else { goto is_not_a_corner; - else - goto is_not_a_corner; - else + } + 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) + } + 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 + } 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 - goto is_not_a_corner; - else - goto is_not_a_corner; - else - goto is_not_a_corner; - else - 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 + } + 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 + } + else { goto is_not_a_corner; - else - goto is_not_a_corner; - else - 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 + } + 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 + } + else { goto is_not_a_corner; - else - goto is_not_a_corner; - else + } + 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 + } + 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 + } + else { goto is_not_a_corner; - else - goto is_not_a_corner; - else + } + else { goto is_not_a_corner; - else - goto is_not_a_corner; - else + } + 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 + } + 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 + } + 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 + } + 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[13]] > cb) - if( p[pixel[14]] > cb) - goto is_a_corner; - else + } + 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 - 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 + } + else { goto is_not_a_corner; - else + } + else { goto is_not_a_corner; - else - goto is_not_a_corner; - else + } + else { goto is_not_a_corner; - else - goto is_not_a_corner; - else + } + 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 + } + 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 + } + 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[11]] > cb) - if( p[pixel[12]] > cb) - goto is_a_corner; - else + } + 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[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 + } + 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 + } + 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 + } + 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 + } + else { goto is_not_a_corner; - else + } + else { goto is_not_a_corner; - else - goto is_not_a_corner; - else + } + 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 + } + 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 - 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 + } + 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 + } + 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 + } + else { goto is_not_a_corner; - else + } + 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 + } + 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; + } + 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; + 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 *fast9_score(const byte *i, int stride, xyFAST *corners, int num_corners, int b) { - int num_corners=0; - xyFAST* ret_corners; - int rsize=512; - int pixel[16]; - int x, y; + int *scores = (int *)malloc(sizeof(int) * num_corners); + int n; - ret_corners = (xyFAST*)malloc(sizeof(xyFAST)*rsize); - make_offsets(pixel, stride); + int pixel[16]; + 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) + 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 - 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[15]] > cb) {} - 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 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 + 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[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 + } + 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 - 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[13]] > cb) + if (p[pixel[14]] > cb) + if (p[pixel[15]] > cb) {} - 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 + else { + continue; + } + else { continue; - else + } + 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 - continue; - else + } + 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 - continue; - else + } + 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 - continue; - else + } + 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[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 + } + 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 - 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 + } + 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 + } + else { continue; - else - continue; - else + } + 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 + } + 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 + } + 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 + } + 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 + } + 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 + } + else { continue; - else - continue; - else + } + 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 + } + 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 - 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 + } + 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 + } + 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 + } + else { continue; - else + } + 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 + } + 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 - 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 + } + 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 + } + else { + continue; + } + else { continue; - else - continue; - else + } + 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 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 + 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 - 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 + } + 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 + } + 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 + } + 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 + } + else { continue; - else - continue; - else + } + 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 + } + 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 + } + else { continue; - else - continue; - else + } + else { continue; - else - continue; - else + } + 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 + } + 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 + } + 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 + } + 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[13]] > cb) - if(p[pixel[14]] > cb) - {} - else + } + 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[13]] > cb) - if(p[pixel[14]] > cb) - if(p[pixel[15]] > cb) - {} - else - continue; - else - continue; - else + } + else { continue; - else + } + 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 + } + else { continue; - else - continue; - else + } + 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 + } + 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 + } + 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[11]] > cb) - if(p[pixel[12]] > cb) - {} - else + } + 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[11]] > cb) - if(p[pixel[12]] > cb) - if(p[pixel[13]] > cb) - {} - else - continue; - else - continue; - else + } + else { continue; - else - if(p[pixel[11]] > cb) - if(p[pixel[12]] > cb) - if(p[pixel[13]] > cb) - if(p[pixel[14]] > cb) - {} - else + } + 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 + } + 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 + } + else { continue; - else + } + else { continue; - else - continue; - else + } + 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 + } + 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 - continue; - else - if(p[pixel[5]] < c_b) - if(p[pixel[6]] < c_b) - if(p[pixel[7]] < c_b) - {} - else + } + 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 + } + 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 + } + 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 + } + else { continue; - else + } + 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 + } + 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; + } + 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 index 88d2571490..683bee37c7 100644 --- a/sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.h +++ b/sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.h @@ -7,16 +7,16 @@ 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 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. + *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. + *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 @@ -34,18 +34,18 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef FAST_H #define FAST_H -typedef struct { int x, y; } xyFAST; +typedef struct { int x, y; } xyFAST; typedef unsigned char byte; -int fast9_corner_score(const byte* p, const int pixel[], int bstart); +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); +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); +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 *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); +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_ardrone.c b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.c index f46243b964..be9529b747 100644 --- a/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.c +++ b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.c @@ -80,7 +80,8 @@ 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 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; //int printed, limit; @@ -305,7 +306,8 @@ int calculateError(int *ImC, int width, int height) 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) +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: @@ -363,7 +365,8 @@ int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int //printf("Subpixel coordinate: (%d,%d)\n\r", p_x[p], p_y[p]); // 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)) { + 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)) { // printf("Outside of ROI, P1[%d,%d]\n\r",p_x[p],p_y[p]); status[p] = 0; } @@ -415,7 +418,10 @@ int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int //printf("it = %d, (p_x+v_x,p_y+v_y) = (%d,%d)\n\r", it, p_x[p]+v_x, p_y[p]+v_y); //printf("it = %d;", it); // 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)) { + 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)) { // printf("Outside of ROI, P1[%d,%d]\n\r",p_x[p],p_y[p]); status[p] = 0; break; diff --git a/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.h b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.h index 14cc568efe..b8acfa2608 100644 --- a/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.h +++ b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.h @@ -30,12 +30,14 @@ #define OPTIC 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 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); +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); diff --git a/sw/airborne/modules/computer_vision/cv/trig.c b/sw/airborne/modules/computer_vision/cv/trig.c index 47110a216f..88a1103f32 100644 --- a/sw/airborne/modules/computer_vision/cv/trig.c +++ b/sw/airborne/modules/computer_vision/cv/trig.c @@ -136,7 +136,8 @@ int atan_zelf(int y, int x) if (x < 0) { x = -x; } if (y < 0) { y = -y; } - flip = 0; if (x < y) { flip = 1; t = x; x = y; y = t; } + flip = 0; + if (x < y) { flip = 1; t = x; x = y; y = t; } if (x == 0) { return 90; } xy = (y * 1000) / x; diff --git a/sw/airborne/modules/computer_vision/lib/paparazzi.h b/sw/airborne/modules/computer_vision/lib/paparazzi.h index f3b2aef8f1..f5f3627e9c 100644 --- a/sw/airborne/modules/computer_vision/lib/paparazzi.h +++ b/sw/airborne/modules/computer_vision/lib/paparazzi.h @@ -26,7 +26,7 @@ inline void paparazzi_message_send(void) { udp_write(sock, (char *) &gst2ppz, sizeof(gst2ppz)); int ret = udp_read(sock, (unsigned char *) &ppz2gst, sizeof(ppz2gst)); - printf("read: %d \n",ret); + printf("read: %d \n", ret); } diff --git a/sw/airborne/modules/computer_vision/lib/udp/socket.c b/sw/airborne/modules/computer_vision/lib/udp/socket.c index 34d73e2023..fba489e282 100644 --- a/sw/airborne/modules/computer_vision/lib/udp/socket.c +++ b/sw/airborne/modules/computer_vision/lib/udp/socket.c @@ -22,7 +22,8 @@ # define SOCKET_ERROR -1 # define IO_SOCKET ioctl -struct UdpSocket *udp_socket(const char *str_ip_out, const int port_out, const int port_in, const int broadcast) { +struct UdpSocket *udp_socket(const char *str_ip_out, const int port_out, const int port_in, const int broadcast) +{ struct UdpSocket *me = malloc(sizeof(struct UdpSocket)); diff --git a/sw/airborne/modules/computer_vision/lib/v4l/video.c b/sw/airborne/modules/computer_vision/lib/v4l/video.c index b3a090ced4..e0a9fb5c31 100644 --- a/sw/airborne/modules/computer_vision/lib/v4l/video.c +++ b/sw/airborne/modules/computer_vision/lib/v4l/video.c @@ -43,10 +43,10 @@ pthread_t video_thread; -void *video_thread_main(void* data); -void *video_thread_main(void* data) +void *video_thread_main(void *data); +void *video_thread_main(void *data) { - struct vid_struct* vid = (struct vid_struct*)data; + struct vid_struct *vid = (struct vid_struct *)data; printf("video_thread_main started\n"); while (1) { fd_set fds; @@ -61,7 +61,7 @@ void *video_thread_main(void* data) r = select(vid->fd + 1, &fds, NULL, NULL, &tv); if (-1 == r) { - if (EINTR == errno) continue; + if (EINTR == errno) { continue; } printf("select err\n"); } @@ -84,13 +84,13 @@ void *video_thread_main(void* data) vid->seq++; - if(vid->trigger) { + if (vid->trigger) { // todo add timestamp again //vid->img->timestamp = util_timestamp(); vid->img->seq = vid->seq; - memcpy(vid->img->buf, vid->buffers[buf.index].buf, vid->w*vid->h*2); - vid->trigger=0; + memcpy(vid->img->buf, vid->buffers[buf.index].buf, vid->w * vid->h * 2); + vid->trigger = 0; } if (ioctl(vid->fd, VIDIOC_QBUF, &buf) < 0) { @@ -108,9 +108,9 @@ int video_init(struct vid_struct *vid) unsigned int i; enum v4l2_buf_type type; - vid->seq=0; - vid->trigger=0; - if(vid->n_buffers==0) vid->n_buffers=4; + vid->seq = 0; + vid->trigger = 0; + if (vid->n_buffers == 0) { vid->n_buffers = 4; } vid->fd = open(vid->device, O_RDWR | O_NONBLOCK, 0); @@ -148,7 +148,7 @@ int video_init(struct vid_struct *vid) printf("Buffer count = %d\n", vid->n_buffers); - vid->buffers = (struct buffer_struct*)calloc(vid->n_buffers, sizeof(struct buffer_struct)); + vid->buffers = (struct buffer_struct *)calloc(vid->n_buffers, sizeof(struct buffer_struct)); for (i = 0; i < vid->n_buffers; ++i) { struct v4l2_buffer buf; @@ -164,11 +164,11 @@ int video_init(struct vid_struct *vid) } vid->buffers[i].length = buf.length; - printf("buffer%d.length=%d\n",i,buf.length); - vid->buffers[i].buf = mmap(NULL, buf.length, PROT_READ|PROT_WRITE, MAP_SHARED, vid->fd, buf.m.offset); + printf("buffer%d.length=%d\n", i, buf.length); + vid->buffers[i].buf = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, vid->fd, buf.m.offset); if (MAP_FAILED == vid->buffers[i].buf) { - printf ("mmap() failed.\n"); + printf("mmap() failed.\n"); return -1; } } @@ -188,14 +188,14 @@ int video_init(struct vid_struct *vid) } type = V4L2_BUF_TYPE_VIDEO_CAPTURE; - if (ioctl(vid->fd, VIDIOC_STREAMON, &type)< 0) { + if (ioctl(vid->fd, VIDIOC_STREAMON, &type) < 0) { printf("ioctl() VIDIOC_STREAMON failed.\n"); return -1; } //start video thread int rc = pthread_create(&video_thread, NULL, video_thread_main, vid); - if(rc) { + if (rc) { printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc); return 202; } @@ -207,27 +207,28 @@ void video_close(struct vid_struct *vid) { int i; for (i = 0; i < (int)vid->n_buffers; ++i) { - if (-1 == munmap(vid->buffers[i].buf, vid->buffers[i].length)) printf("munmap() failed.\n"); + if (-1 == munmap(vid->buffers[i].buf, vid->buffers[i].length)) { printf("munmap() failed.\n"); } } close(vid->fd); } struct img_struct *video_create_image(struct vid_struct *vid) { - struct img_struct* img = (struct img_struct*)malloc(sizeof(struct img_struct)); - img->w=vid->w; - img->h=vid->h; - img->buf = (unsigned char*)malloc(vid->h*vid->w*2); + struct img_struct *img = (struct img_struct *)malloc(sizeof(struct img_struct)); + img->w = vid->w; + img->h = vid->h; + img->buf = (unsigned char *)malloc(vid->h * vid->w * 2); return img; } pthread_mutex_t video_grab_mutex = PTHREAD_MUTEX_INITIALIZER; -void video_grab_image(struct vid_struct *vid, struct img_struct *img) { +void video_grab_image(struct vid_struct *vid, struct img_struct *img) +{ pthread_mutex_lock(&video_grab_mutex); vid->img = img; - vid->trigger=1; + vid->trigger = 1; // while(vid->trigger) pthread_yield(); - while(vid->trigger) usleep(1); + while (vid->trigger) { usleep(1); } pthread_mutex_unlock(&video_grab_mutex); } diff --git a/sw/airborne/modules/computer_vision/lib/v4l/video.h b/sw/airborne/modules/computer_vision/lib/v4l/video.h index 125892ad1b..351ba68522 100644 --- a/sw/airborne/modules/computer_vision/lib/v4l/video.h +++ b/sw/airborne/modules/computer_vision/lib/v4l/video.h @@ -25,7 +25,7 @@ #include "../../cv/image.h" struct buffer_struct { - void * buf; + void *buf; size_t length; }; @@ -39,7 +39,7 @@ struct vid_struct { //private members int trigger; struct img_struct *img; - struct buffer_struct * buffers; + struct buffer_struct *buffers; int fd; }; diff --git a/sw/airborne/modules/computer_vision/opticflow/dummy.c b/sw/airborne/modules/computer_vision/opticflow/dummy.c index 63978a0e13..8aa682a68e 100644 --- a/sw/airborne/modules/computer_vision/opticflow/dummy.c +++ b/sw/airborne/modules/computer_vision/opticflow/dummy.c @@ -1,3 +1,4 @@ -void dummyFunction(void) { - return; +void dummyFunction(void) +{ + return; } diff --git a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c index 0acb8b7674..1d6db95128 100644 --- a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c +++ b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c @@ -65,102 +65,85 @@ float Vely_Int; float Error_Velx; float Error_Vely; -#define CMD_OF_SAT 1500 // 40 deg = 2859.1851 +#define CMD_OF_SAT 1500 // 40 deg = 2859.1851 unsigned char saturateX = 0, saturateY = 0; unsigned int set_heading; void init_hover_stabilization_onvision() { - INT_EULERS_ZERO(cmd_euler); + 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; + 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; + set_heading = 1; - Error_Velx = 0; - Error_Vely = 0; - Velx_Int = 0; - Vely_Int = 0; + Error_Velx = 0; + Error_Vely = 0; + Velx_Int = 0; + Vely_Int = 0; } void run_hover_stabilization_onvision(void) { - if(autopilot_mode == AP_MODE_VISION_HOVER) - { - run_opticflow_hover(); - } - else - { - Velx_Int = 0; - Vely_Int = 0; - } + if (autopilot_mode == AP_MODE_VISION_HOVER) { + run_opticflow_hover(); + } else { + Velx_Int = 0; + Vely_Int = 0; + } } void run_opticflow_hover(void) { - if(flow_count) - { - Error_Velx = Velx - vision_desired_vx; - Error_Vely = Vely - vision_desired_vy; - } - else - { - Error_Velx = 0; - Error_Vely = 0; - } + if (flow_count) { + Error_Velx = Velx - vision_desired_vx; + Error_Vely = 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 (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 (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); - } + 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;} + 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, &Velx, &Vely, &Velx_Int, &Vely_Int, &cmd_euler.phi, &cmd_euler.theta); + stabilization_attitude_set_rpy_setpoint_i(&cmd_euler); + DOWNLINK_SEND_VISION_STABILIZATION(DefaultChannel, DefaultDevice, &Velx, &Vely, &Velx_Int, &Vely_Int, &cmd_euler.phi, + &cmd_euler.theta); } diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_code.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_code.c index ee624f2a43..832e8bf864 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_code.c +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_code.c @@ -95,238 +95,216 @@ struct FloatVect3 V_body; // Called by plugin void my_plugin_init(void) { - // Initialize variables - gray_frame = (unsigned char *) calloc(imgWidth*imgHeight,sizeof(unsigned char)); - prev_frame = (unsigned char *) calloc(imgWidth*imgHeight*2,sizeof(unsigned char)); - prev_gray_frame = (unsigned char *) calloc(imgWidth*imgHeight,sizeof(unsigned char)); - x = (int *) calloc(MAX_COUNT,sizeof(int)); - new_x = (int *) calloc(MAX_COUNT,sizeof(int)); - y = (int *) calloc(MAX_COUNT,sizeof(int)); - new_y = (int *) calloc(MAX_COUNT,sizeof(int)); - status = (int *) calloc(MAX_COUNT,sizeof(int)); - dx = (int *) calloc(MAX_COUNT,sizeof(int)); - dy = (int *) calloc(MAX_COUNT,sizeof(int)); - old_img_init = 1; - OFx = 0.0; - OFy = 0.0; - dx_sum = 0.0; - dy_sum = 0.0; - diff_roll = 0.0; - diff_pitch = 0.0; - cam_h = 0.0; - prev_pitch = 0.0; - prev_roll = 0.0; - curr_pitch = 0.0; - curr_roll = 0.0; - curr_yaw = 0.0; - OFx_trans = 0.0; - OFy_trans = 0.0; - Velx = 0.0; - Vely = 0.0; + // Initialize variables + gray_frame = (unsigned char *) calloc(imgWidth * imgHeight, sizeof(unsigned char)); + prev_frame = (unsigned char *) calloc(imgWidth * imgHeight * 2, sizeof(unsigned char)); + prev_gray_frame = (unsigned char *) calloc(imgWidth * imgHeight, sizeof(unsigned char)); + x = (int *) calloc(MAX_COUNT, sizeof(int)); + new_x = (int *) calloc(MAX_COUNT, sizeof(int)); + y = (int *) calloc(MAX_COUNT, sizeof(int)); + new_y = (int *) calloc(MAX_COUNT, sizeof(int)); + status = (int *) calloc(MAX_COUNT, sizeof(int)); + dx = (int *) calloc(MAX_COUNT, sizeof(int)); + dy = (int *) calloc(MAX_COUNT, sizeof(int)); + old_img_init = 1; + OFx = 0.0; + OFy = 0.0; + dx_sum = 0.0; + dy_sum = 0.0; + diff_roll = 0.0; + diff_pitch = 0.0; + cam_h = 0.0; + prev_pitch = 0.0; + prev_roll = 0.0; + curr_pitch = 0.0; + curr_roll = 0.0; + curr_yaw = 0.0; + OFx_trans = 0.0; + OFy_trans = 0.0; + Velx = 0.0; + Vely = 0.0; } void my_plugin_run(unsigned char *frame) { - if(old_img_init == 1) - { - memcpy(prev_frame,frame,imgHeight*imgWidth*2); - CvtYUYV2Gray(prev_gray_frame, prev_frame, imgWidth, imgHeight); - old_img_init = 0; - } + if (old_img_init == 1) { + memcpy(prev_frame, frame, imgHeight * imgWidth * 2); + CvtYUYV2Gray(prev_gray_frame, prev_frame, imgWidth, imgHeight); + old_img_init = 0; + } - // *********************************************************************************************************************** - // Additional information from other sensors - // *********************************************************************************************************************** + // *********************************************************************************************************************** + // Additional information from other sensors + // *********************************************************************************************************************** - // Compute body velocities from ENU - V_Ned.x = stateGetSpeedNed_f()->x; - V_Ned.y = stateGetSpeedNed_f()->y; - V_Ned.z = stateGetSpeedNed_f()->z; + // Compute body velocities from ENU + V_Ned.x = stateGetSpeedNed_f()->x; + V_Ned.y = stateGetSpeedNed_f()->y; + V_Ned.z = stateGetSpeedNed_f()->z; - struct FloatQuat* BodyQuaternions = stateGetNedToBodyQuat_f(); - FLOAT_RMAT_OF_QUAT(Rmat_Ned2Body,*BodyQuaternions); - RMAT_VECT3_MUL(V_body, Rmat_Ned2Body, V_Ned); + struct FloatQuat *BodyQuaternions = stateGetNedToBodyQuat_f(); + FLOAT_RMAT_OF_QUAT(Rmat_Ned2Body, *BodyQuaternions); + RMAT_VECT3_MUL(V_body, Rmat_Ned2Body, V_Ned); - // *********************************************************************************************************************** - // Corner detection - // *********************************************************************************************************************** + // *********************************************************************************************************************** + // Corner detection + // *********************************************************************************************************************** - // FAST corner detection - int fast_threshold = 20; - xyFAST* pnts_fast; - pnts_fast = fast9_detect((const byte*)prev_gray_frame, imgWidth, imgHeight, imgWidth, fast_threshold, &count); + // FAST corner detection + int fast_threshold = 20; + xyFAST *pnts_fast; + pnts_fast = fast9_detect((const byte *)prev_gray_frame, imgWidth, imgHeight, imgWidth, fast_threshold, &count); - if(count > MAX_COUNT) count = MAX_COUNT; - for(int i = 0; i < count; i++) - { - x[i] = pnts_fast[i].x; - y[i] = pnts_fast[i].y; - } - free(pnts_fast); + if (count > MAX_COUNT) { count = MAX_COUNT; } + for (int i = 0; i < count; i++) { + x[i] = pnts_fast[i].x; + y[i] = pnts_fast[i].y; + } + free(pnts_fast); - // Remove neighbouring corners - min_distance = 3; - min_distance2 = min_distance*min_distance; - int *labelmin; - labelmin = (int *) calloc(MAX_COUNT,sizeof(int)); - for(int i = 0; i < count; i++) - { - for(int j = i+1; j < count; j++) - { - // distance squared: - distance2 = (x[i] - x[j])*(x[i] - x[j]) + (y[i] - y[j])*(y[i] - y[j]); - if(distance2 < min_distance2) - { - labelmin[i] = 1; - } - } - } + // Remove neighbouring corners + min_distance = 3; + min_distance2 = min_distance * min_distance; + int *labelmin; + labelmin = (int *) calloc(MAX_COUNT, sizeof(int)); + for (int i = 0; i < count; i++) { + for (int j = i + 1; j < count; j++) { + // distance squared: + 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 = count; - for(int i = count-1; i >= 0; i-- ) - { - remove_point = 0; + int count_fil = count; + for (int i = count - 1; i >= 0; i--) { + remove_point = 0; - if(labelmin[i]) - { - remove_point = 1; - } + if (labelmin[i]) { + remove_point = 1; + } - if(remove_point) - { - for(c = i; c max_count) count_fil = max_count; - count = count_fil; - free(labelmin); + if (count_fil > max_count) { count_fil = max_count; } + count = count_fil; + free(labelmin); - // ********************************************************************************************************************** - // Corner Tracking - // ********************************************************************************************************************** - CvtYUYV2Gray(gray_frame, frame, imgWidth, imgHeight); + // ********************************************************************************************************************** + // Corner Tracking + // ********************************************************************************************************************** + CvtYUYV2Gray(gray_frame, frame, imgWidth, imgHeight); - error_opticflow = opticFlowLK(gray_frame, prev_gray_frame, x, y, count_fil, imgWidth, imgHeight, new_x, new_y, status, 5, 100); + error_opticflow = opticFlowLK(gray_frame, prev_gray_frame, x, y, count_fil, imgWidth, imgHeight, new_x, new_y, status, + 5, 100); - flow_count = count_fil; - for(int i=count_fil-1; i>=0; i--) - { - remove_point = 1; + flow_count = count_fil; + for (int i = count_fil - 1; i >= 0; i--) { + remove_point = 1; - if(status[i] && !(new_x[i] < borderx || new_x[i] > (imgWidth-1-borderx) || - new_y[i] < bordery || new_y[i] > (imgHeight-1-bordery))) - { - remove_point = 0; - } + if (status[i] && !(new_x[i] < borderx || new_x[i] > (imgWidth - 1 - borderx) || + new_y[i] < bordery || new_y[i] > (imgHeight - 1 - bordery))) { + remove_point = 0; + } - if(remove_point) - { - for(c = i; c theta; - curr_roll = stateGetNedToBodyEulers_f()->phi; - curr_yaw = stateGetNedToBodyEulers_f()->psi; + // Flow Derotation + curr_pitch = stateGetNedToBodyEulers_f()->theta; + curr_roll = stateGetNedToBodyEulers_f()->phi; + curr_yaw = stateGetNedToBodyEulers_f()->psi; - diff_pitch = (curr_pitch - prev_pitch)*imgHeight/FOV_H; - diff_roll = (curr_roll - prev_roll)*imgWidth/FOV_W; + diff_pitch = (curr_pitch - prev_pitch) * imgHeight / FOV_H; + diff_roll = (curr_roll - prev_roll) * imgWidth / FOV_W; - prev_pitch = curr_pitch; - prev_roll = curr_roll; + prev_pitch = curr_pitch; + prev_roll = curr_roll; #ifdef FLOW_DEROTATION - if(flow_count) - { - OFx_trans = dx_sum - diff_roll; - OFy_trans = dy_sum - diff_pitch; + if (flow_count) { + OFx_trans = dx_sum - diff_roll; + OFy_trans = dy_sum - diff_pitch; - if((OFx_trans<=0) != (dx_sum<=0)) - { - OFx_trans = 0; - OFy_trans = 0; - } - } - else - { - OFx_trans = dx_sum; - OFy_trans = dy_sum; - } + if ((OFx_trans <= 0) != (dx_sum <= 0)) { + OFx_trans = 0; + OFy_trans = 0; + } + } else { + OFx_trans = dx_sum; + OFy_trans = dy_sum; + } #else - OFx_trans = dx_sum; - OFy_trans = dy_sum; + OFx_trans = dx_sum; + OFy_trans = dy_sum; #endif - // Average Filter - OFfilter(&OFx, &OFy, OFx_trans, OFy_trans, flow_count, 1); + // Average Filter + OFfilter(&OFx, &OFy, OFx_trans, OFy_trans, flow_count, 1); - // Velocity Computation - #ifdef USE_SONAR - cam_h = 1; //ins_impl.sonar_z; - #else - cam_h = 1; - #endif + // Velocity Computation +#ifdef USE_SONAR + cam_h = 1; //ins_impl.sonar_z; +#else + cam_h = 1; +#endif - if(flow_count) - { - Velx = OFy*cam_h*FPS/Fy_ARdrone + 0.05; - Vely = -OFx*cam_h*FPS/Fx_ARdrone - 0.1; - } - else - { - Velx = 0.0; - Vely = 0.0; - } + if (flow_count) { + Velx = OFy * cam_h * FPS / Fy_ARdrone + 0.05; + Vely = -OFx * cam_h * FPS / Fx_ARdrone - 0.1; + } else { + Velx = 0.0; + Vely = 0.0; + } - // ********************************************************************************************************************** - // Next Loop Preparation - // ********************************************************************************************************************** + // ********************************************************************************************************************** + // Next Loop Preparation + // ********************************************************************************************************************** - memcpy(prev_frame,frame,imgHeight*imgWidth*2); - memcpy(prev_gray_frame,gray_frame,imgHeight*imgWidth); + memcpy(prev_frame, frame, imgHeight * imgWidth * 2); + memcpy(prev_gray_frame, gray_frame, imgHeight * imgWidth); - // ********************************************************************************************************************** - // Downlink Message - // ********************************************************************************************************************** - DOWNLINK_SEND_OF_HOVER(DefaultChannel, DefaultDevice, &FPS, &dx_sum, &dy_sum, &OFx, &OFy, &diff_roll, &diff_pitch, &Velx, &Vely, &V_body.x, &V_body.y, &cam_h, &count); + // ********************************************************************************************************************** + // Downlink Message + // ********************************************************************************************************************** + DOWNLINK_SEND_OF_HOVER(DefaultChannel, DefaultDevice, &FPS, &dx_sum, &dy_sum, &OFx, &OFy, &diff_roll, &diff_pitch, + &Velx, &Vely, &V_body.x, &V_body.y, &cam_h, &count); } diff --git a/sw/airborne/modules/computer_vision/opticflow/video_message_structs.h b/sw/airborne/modules/computer_vision/opticflow/video_message_structs.h index 2039dd4edd..7974595d86 100644 --- a/sw/airborne/modules/computer_vision/opticflow/video_message_structs.h +++ b/sw/airborne/modules/computer_vision/opticflow/video_message_structs.h @@ -8,19 +8,17 @@ #define N_BINS 10 -struct gst2ppz_message_struct -{ +struct gst2ppz_message_struct { unsigned int ID; // Keep different modules for using each others data - unsigned int counter; // counter to keep track of data - unsigned int obstacle_bins[N_BINS]; // optical flow output, shift in x direction - unsigned int uncertainty_bins[N_BINS]; //optical flow output, shift in y direction + unsigned int counter; // counter to keep track of data + unsigned int obstacle_bins[N_BINS]; // optical flow output, shift in x direction + unsigned int uncertainty_bins[N_BINS]; //optical flow output, shift in y direction }; extern struct gst2ppz_message_struct gst2ppz; -struct ppz2gst_message_struct -{ +struct ppz2gst_message_struct { unsigned int ID; // Keep different modules for using each others data - unsigned int counter; //counter to keep track of data + unsigned int counter; //counter to keep track of data int pitch; int roll; int alt; diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c index 845497f896..0a20b5a550 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -45,42 +45,43 @@ float FPS; volatile long timestamp; #define USEC_PER_SEC 1000000L -long time_elapsed (struct timeval *t1, struct timeval *t2) +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; + 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; } struct timeval start_time; struct timeval end_time; -void start_timer() { - gettimeofday (&start_time, NULL); +void start_timer() +{ + gettimeofday(&start_time, NULL); } -long end_timer() { - gettimeofday (&end_time, NULL); - return time_elapsed(&start_time, &end_time); +long end_timer() +{ + gettimeofday(&end_time, NULL); + return time_elapsed(&start_time, &end_time); } void opticflow_module_init(void) { - // Immediately start the vision thread when the module initialized - opticflow_module_start(); + // Immediately start the vision thread when the module initialized + opticflow_module_start(); - // Stabilization Code Initialization - init_hover_stabilization_onvision(); + // Stabilization Code Initialization + init_hover_stabilization_onvision(); - // Frame Rate Initialization - FPS = 0.0; - timestamp=0; - start_timer(); + // Frame Rate Initialization + FPS = 0.0; + timestamp = 0; + start_timer(); } @@ -90,8 +91,7 @@ void opticflow_module_run(void) { // Read Latest Vision Module Results - if (computervision_thread_has_results) - { + if (computervision_thread_has_results) { computervision_thread_has_results = 0; run_hover_stabilization_onvision(); } @@ -120,98 +120,97 @@ void opticflow_module_run(void) pthread_t computervision_thread; volatile uint8_t computervision_thread_status = 0; volatile uint8_t computer_vision_thread_command = 0; -void *computervision_thread_main(void* data); -void *computervision_thread_main(void* data) +void *computervision_thread_main(void *data); +void *computervision_thread_main(void *data) { - // Video Input - struct vid_struct vid; - vid.device = (char*)"/dev/video2"; // video1 = front camera; video2 = bottom camera - vid.w=320; - vid.h=240; - vid.n_buffers = 4; + // Video Input + struct vid_struct vid; + vid.device = (char *)"/dev/video2"; // video1 = front camera; video2 = bottom camera + vid.w = 320; + vid.h = 240; + vid.n_buffers = 4; - if (video_init(&vid)<0) { - printf("Error initialising video\n"); - computervision_thread_status = -1; - return 0; - } + if (video_init(&vid) < 0) { + printf("Error initialising video\n"); + computervision_thread_status = -1; + return 0; + } - // Video Grabbing - struct img_struct* img_new = video_create_image(&vid); + // Video Grabbing + struct img_struct *img_new = video_create_image(&vid); - // Video Resizing - #define DOWNSIZE_FACTOR 1 - struct img_struct small; - small.w = vid.w / DOWNSIZE_FACTOR; - small.h = vid.h / DOWNSIZE_FACTOR; - small.buf = (uint8_t*)malloc(small.w*small.h*2); + // Video Resizing +#define DOWNSIZE_FACTOR 1 + struct img_struct small; + small.w = vid.w / DOWNSIZE_FACTOR; + small.h = vid.h / DOWNSIZE_FACTOR; + small.buf = (uint8_t *)malloc(small.w * small.h * 2); #ifdef DOWNLINK_VIDEO - // Video Compression - uint8_t* jpegbuf = (uint8_t*)malloc(vid.h*vid.w*2); + // 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); + // 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 - imgWidth = small.w; - imgHeight = small.h; - verbose = 2; - my_plugin_init(); + // First Apply Settings before init + imgWidth = small.w; + imgHeight = small.h; + verbose = 2; + my_plugin_init(); - while (computer_vision_thread_command > 0) - { - video_grab_image(&vid, img_new); + while (computer_vision_thread_command > 0) { + video_grab_image(&vid, img_new); - // FPS - timestamp = end_timer(); - FPS = (float) 1000000/(float)timestamp; - // printf("dt = %d, FPS = %f\n",timestamp, FPS); - start_timer(); + // FPS + timestamp = end_timer(); + FPS = (float) 1000000 / (float)timestamp; + // printf("dt = %d, FPS = %f\n",timestamp, FPS); + start_timer(); - // Resize - //resize_uyuv(img_new, &small, DOWNSIZE_FACTOR); + // Resize + //resize_uyuv(img_new, &small, DOWNSIZE_FACTOR); - // Run Image Processing - my_plugin_run(img_new->buf); - // my_plugin_run(small.buf); + // Run Image Processing + my_plugin_run(img_new->buf); + // my_plugin_run(small.buf); - #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); +#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); - uint32_t delta_t_per_frame = 0; // 0 = use drone clock - send_rtp_frame(vsock, jpegbuf,size, small.w, small.h,0, quality_factor, dri_header, delta_t_per_frame); - #endif - computervision_thread_has_results++; - } - printf("Thread Closed\n"); - video_close(&vid); - computervision_thread_status = -100; - return 0; + printf("Sending an image ...%u\n", size); + uint32_t delta_t_per_frame = 0; // 0 = use drone clock + send_rtp_frame(vsock, jpegbuf, size, small.w, small.h, 0, quality_factor, dri_header, delta_t_per_frame); +#endif + computervision_thread_has_results++; + } + printf("Thread Closed\n"); + video_close(&vid); + computervision_thread_status = -100; + return 0; } void opticflow_module_start(void) { - computer_vision_thread_command = 1; - int rc = pthread_create(&computervision_thread, NULL, computervision_thread_main, NULL); - if(rc) { - printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc); - } + computer_vision_thread_command = 1; + int rc = pthread_create(&computervision_thread, NULL, computervision_thread_main, NULL); + if (rc) { + printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc); + } } void opticflow_module_stop(void) { - computer_vision_thread_command = 0; + computer_vision_thread_command = 0; } From 535e09c210090341d4167e14ad56a1c40e9a8c02 Mon Sep 17 00:00:00 2001 From: dewagter Date: Mon, 12 Jan 2015 22:04:16 +0100 Subject: [PATCH 04/99] [opticflow] docs and settings --- conf/modules/cv_opticflow.xml | 23 ++++++++++++++++++----- 1 file changed, 18 insertions(+), 5 deletions(-) diff --git a/conf/modules/cv_opticflow.xml b/conf/modules/cv_opticflow.xml index 5493c8db3c..9067067c3d 100644 --- a/conf/modules/cv_opticflow.xml +++ b/conf/modules/cv_opticflow.xml @@ -2,16 +2,29 @@ - Video ARDone 2 + + 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 + - - - - + + + + + + + + +
From 071237678354a63cf5b72840e032c49beab99e0e Mon Sep 17 00:00:00 2001 From: dewagter Date: Mon, 12 Jan 2015 22:11:12 +0100 Subject: [PATCH 05/99] [opticflow] remove unused --- sw/airborne/modules/computer_vision/lib/v4l/ardrone2.c | 0 sw/airborne/modules/computer_vision/lib/v4l/ardrone2.h | 0 sw/airborne/modules/computer_vision/opticflow/dummy.c | 4 ---- 3 files changed, 4 deletions(-) delete mode 100644 sw/airborne/modules/computer_vision/lib/v4l/ardrone2.c delete mode 100644 sw/airborne/modules/computer_vision/lib/v4l/ardrone2.h delete mode 100644 sw/airborne/modules/computer_vision/opticflow/dummy.c diff --git a/sw/airborne/modules/computer_vision/lib/v4l/ardrone2.c b/sw/airborne/modules/computer_vision/lib/v4l/ardrone2.c deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/sw/airborne/modules/computer_vision/lib/v4l/ardrone2.h b/sw/airborne/modules/computer_vision/lib/v4l/ardrone2.h deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/sw/airborne/modules/computer_vision/opticflow/dummy.c b/sw/airborne/modules/computer_vision/opticflow/dummy.c deleted file mode 100644 index 8aa682a68e..0000000000 --- a/sw/airborne/modules/computer_vision/opticflow/dummy.c +++ /dev/null @@ -1,4 +0,0 @@ -void dummyFunction(void) -{ - return; -} From b4ae64c6b3902fe1a703bf4d25c2cefc55e4d981 Mon Sep 17 00:00:00 2001 From: dewagter Date: Tue, 13 Jan 2015 19:26:15 +0100 Subject: [PATCH 06/99] [opticflow] messages --- conf/messages.xml | 26 ++++++++++++++++++++++++-- 1 file changed, 24 insertions(+), 2 deletions(-) diff --git a/conf/messages.xml b/conf/messages.xml index 3613efc8d1..6ab44cd8ae 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -1972,8 +1972,30 @@ - - + + + + + + + + + + + + + + + + + + + + + + + + From 57b1453f1dcb8cb583194854a72710faac876794 Mon Sep 17 00:00:00 2001 From: dewagter Date: Tue, 13 Jan 2015 19:36:46 +0100 Subject: [PATCH 07/99] [opticflow] airframe --- conf/airframes/ardrone2_opticflow_hover.xml | 230 ++++++++++++++++++++ 1 file changed, 230 insertions(+) create mode 100644 conf/airframes/ardrone2_opticflow_hover.xml diff --git a/conf/airframes/ardrone2_opticflow_hover.xml b/conf/airframes/ardrone2_opticflow_hover.xml new file mode 100644 index 0000000000..dfb6a69038 --- /dev/null +++ b/conf/airframes/ardrone2_opticflow_hover.xml @@ -0,0 +1,230 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + +
+ + + + + + + + + +
+ + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + +
+ + + +
+ + + + + + +
+ +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + + + + + +
+ +
+ + + +
+ +
+ + + + +
+ +
+ + + + + +
+ From 306d04ef5f7a5a1a58167ff46827b043aa2c28e1 Mon Sep 17 00:00:00 2001 From: dewagter Date: Tue, 13 Jan 2015 19:46:28 +0100 Subject: [PATCH 08/99] [opticflow] cleanup --- .../cv/opticflow/optic_flow_ardrone.c | 4 +- .../modules/computer_vision/lib/tcp/socket.c | 139 ------------------ .../modules/computer_vision/lib/tcp/socket.h | 17 --- .../opticflow/video_message_structs.h | 30 ---- 4 files changed, 2 insertions(+), 188 deletions(-) delete mode 100644 sw/airborne/modules/computer_vision/lib/tcp/socket.c delete mode 100644 sw/airborne/modules/computer_vision/lib/tcp/socket.h delete mode 100644 sw/airborne/modules/computer_vision/opticflow/video_message_structs.h diff --git a/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.c b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.c index be9529b747..768758729d 100644 --- a/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.c +++ b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.c @@ -34,8 +34,8 @@ #include #include #include -#include "optic_flow_gdc.h" -#include "../../modules/OpticFlow/opticflow_module.h" +#include "optic_flow_ardrone.h" +#include "../opticflow_module.h" #define int_index(x,y) (y * IMG_WIDTH + x) #define uint_index(xx, yy) (((yy * IMG_WIDTH + xx) * 2) & 0xFFFFFFFC) diff --git a/sw/airborne/modules/computer_vision/lib/tcp/socket.c b/sw/airborne/modules/computer_vision/lib/tcp/socket.c deleted file mode 100644 index 9479c698fa..0000000000 --- a/sw/airborne/modules/computer_vision/lib/tcp/socket.c +++ /dev/null @@ -1,139 +0,0 @@ -#include "socket.h" -#include /* socket definitions */ -#include /* socket types */ -#include /* inet (3) funtions */ -#include /* misc. UNIX functions */ -#include -#include /* memset */ - -#include -#include - - - - -/* Global constants */ - -#define ECHO_PORT (2002) -#define MAX_LINE (1000) - -/* Global variables */ -int list_s; /* listening socket */ -int conn_s; /* connection socket */ -struct sockaddr_in servaddr; /* socket address structure */ -char buffer[MAX_LINE]; /* character buffer */ -char *endptr; /* for strtol() */ - -int closeSocket(void) -{ - return close(conn_s); -} - -int initSocket(void) -{ - - /* Create the listening socket */ - - if ((list_s = socket(AF_INET, SOCK_STREAM, 0)) < 0) { - fprintf(stderr, "ECHOSERV: Error creating listening socket.\n"); - return -1; - } - - - /* Set all bytes in socket address structure to - zero, and fill in the relevant data members */ - - memset(&servaddr, 0, sizeof(servaddr)); - servaddr.sin_family = AF_INET; - servaddr.sin_addr.s_addr = htonl(INADDR_ANY); - servaddr.sin_port = htons(ECHO_PORT); - - /* Bind our socket addresss to the - listening socket, and call listen() */ - - if (bind(list_s, (struct sockaddr *) &servaddr, sizeof(servaddr)) < 0) { - fprintf(stderr, "ECHOSERV: Error calling bind()\n"); - exit(EXIT_FAILURE); - } - - if (listen(list_s, LISTENQ) < 0) { - fprintf(stderr, "ECHOSERV: Error calling listen()\n"); - exit(EXIT_FAILURE); - } - - - - /* Wait for a connection, then accept() it */ - if ((conn_s = accept(list_s, NULL, NULL)) < 0) { - fprintf(stderr, "ECHOSERV: Error calling function accept()\n"); - return -1; - } - printf("Connected!\n"); - return 1; -} - -/* Read a line from a socket */ - -ssize_t Readline_socket(void *vptr, size_t maxlen) -{ - ssize_t n, rc; - char c, *buffer; - - buffer = vptr; - - for (n = 1; n < maxlen; n++) { - - if ((rc = read(conn_s, &c, 1)) == 1) { - *buffer++ = c; - if (c == '\n') { - break; - } - } else if (rc == 0) { - if (n == 1) { - return 0; - } else { - break; - } - } else { - if (errno == EINTR) { - continue; - } - return -1; - } - } - - *buffer = 0; - return n; -} - - -/* Write a line to a socket */ - -ssize_t Writeline_socket(struct img_struct *img, size_t n) -{ - size_t nleft; - ssize_t nwritten; - unsigned char *buf1 = img->buf; - nleft = n; - - //printf("socket buf1 %d, %d, %d, %d, %d, %d\n",buf1[0],buf1[1],buf1[2],buf1[3],buf1[4],buf1[5]); - - while (nleft > 0) { - if ((nwritten = write(conn_s, buf1, nleft)) <= 0) { - if (errno == EINTR) { - nwritten = 0; - } else { - return -1; - } - } - nleft -= nwritten; - buf1 += nwritten; - } - - return n; - -} - - - - diff --git a/sw/airborne/modules/computer_vision/lib/tcp/socket.h b/sw/airborne/modules/computer_vision/lib/tcp/socket.h deleted file mode 100644 index 04091cd484..0000000000 --- a/sw/airborne/modules/computer_vision/lib/tcp/socket.h +++ /dev/null @@ -1,17 +0,0 @@ -#ifndef SOCKET_H -#define SOCKET_H - -#include /* for ssize_t data type */ -#include "./video/video.h" - -#define LISTENQ (1024) /* Backlog for listen() */ - - -/* Function declarations */ -int initSocket(void) ; -ssize_t Readline(void *vptr, size_t maxlen); -ssize_t Writeline(struct img_struct *img, size_t maxlen); -int closeSocket(void); - -#endif /* SOCKET_H */ - diff --git a/sw/airborne/modules/computer_vision/opticflow/video_message_structs.h b/sw/airborne/modules/computer_vision/opticflow/video_message_structs.h deleted file mode 100644 index 7974595d86..0000000000 --- a/sw/airborne/modules/computer_vision/opticflow/video_message_structs.h +++ /dev/null @@ -1,30 +0,0 @@ -#ifndef VMSTRTS_H -#define VMSTRTS_H - -/*** - an exact copy of this file to exist in ppz - this files keeps the structs that are serialized and streamed over tcp/ip through localhost - */ - -#define N_BINS 10 - -struct gst2ppz_message_struct { - unsigned int ID; // Keep different modules for using each others data - unsigned int counter; // counter to keep track of data - unsigned int obstacle_bins[N_BINS]; // optical flow output, shift in x direction - unsigned int uncertainty_bins[N_BINS]; //optical flow output, shift in y direction -}; -extern struct gst2ppz_message_struct gst2ppz; - -struct ppz2gst_message_struct { - unsigned int ID; // Keep different modules for using each others data - unsigned int counter; //counter to keep track of data - int pitch; - int roll; - int alt; - int adjust_factor; // 0-10 :adjust brightness -}; -extern struct ppz2gst_message_struct ppz2gst; - -#endif /* VMSTRTS_H */ - From e3377aad6f9c445a9f9f6be6abb7908c785b230d Mon Sep 17 00:00:00 2001 From: dewagter Date: Tue, 13 Jan 2015 19:46:53 +0100 Subject: [PATCH 09/99] [opticflow] compile --- .../modules/computer_vision/opticflow/hover_stabilization.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c index 1d6db95128..2929305a37 100644 --- a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c +++ b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c @@ -69,6 +69,10 @@ float Error_Vely; unsigned char saturateX = 0, saturateY = 0; unsigned int set_heading; +// TODO FIX +#define AP_MODE_VISION_HOVER 3 + + void init_hover_stabilization_onvision() { INT_EULERS_ZERO(cmd_euler); From 98c27f257cfbcfe6f955a3810402c0e014587e11 Mon Sep 17 00:00:00 2001 From: dewagter Date: Tue, 13 Jan 2015 20:26:51 +0100 Subject: [PATCH 10/99] [opticflow] controller --- conf/messages.xml | 2 +- conf/modules/cv_opticflow.xml | 1 + sw/airborne/firmwares/rotorcraft/autopilot.c | 6 ++++++ sw/airborne/firmwares/rotorcraft/autopilot.h | 1 + .../firmwares/rotorcraft/guidance/guidance_h.c | 11 +++++++++++ .../firmwares/rotorcraft/guidance/guidance_h.h | 17 +++++++++-------- .../firmwares/rotorcraft/guidance/guidance_v.c | 14 ++++++++++++++ .../firmwares/rotorcraft/guidance/guidance_v.h | 13 +++++++------ .../opticflow/hover_stabilization.c | 5 +---- .../modules/computer_vision/opticflow_module.c | 6 ++++++ .../modules/computer_vision/opticflow_module.h | 4 ++++ 11 files changed, 61 insertions(+), 19 deletions(-) diff --git a/conf/messages.xml b/conf/messages.xml index 6ab44cd8ae..42c451aba9 100644 --- a/conf/messages.xml +++ b/conf/messages.xml @@ -2019,7 +2019,7 @@ - + diff --git a/conf/modules/cv_opticflow.xml b/conf/modules/cv_opticflow.xml index 9067067c3d..146da49cad 100644 --- a/conf/modules/cv_opticflow.xml +++ b/conf/modules/cv_opticflow.xml @@ -37,6 +37,7 @@ + diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index e55f24fe6d..cae56817ba 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -423,6 +423,9 @@ 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_OUTERLOOP: + guidance_h_mode_changed(GUIDANCE_H_MODE_MODULE_OUTERLOOP); + break; default: break; } @@ -464,6 +467,9 @@ 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_OUTERLOOP: + guidance_v_mode_changed(GUIDANCE_V_MODE_MODULE_OUTERLOOP); + break; default: break; } diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.h b/sw/airborne/firmwares/rotorcraft/autopilot.h index 75a52806e7..cad980c508 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_OUTERLOOP 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..4b56a2f7c2 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -252,6 +252,7 @@ void guidance_h_mode_changed(uint8_t new_mode) stabilization_attitude_enter(); break; + case GUIDANCE_H_MODE_MODULE_OUTERLOOP: case GUIDANCE_H_MODE_NAV: guidance_h_nav_enter(); #if NO_ATTITUDE_RESET_ON_MODE_CHANGE @@ -304,6 +305,7 @@ void guidance_h_read_rc(bool_t in_flight) #endif break; + case GUIDANCE_H_MODE_MODULE_OUTERLOOP: 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); @@ -386,6 +388,15 @@ void guidance_h_run(bool_t in_flight) } stabilization_attitude_run(in_flight); break; + case GUIDANCE_H_MODE_MODULE_OUTERLOOP: + if (!in_flight) { + guidance_h_nav_enter(); +#if USE_MODULE_OUTERLOOP==1 + guidance_module_run(in_flight); +#endif + } + + break; default: break; diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h index ac9b064017..c9cd7ff394 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h @@ -49,14 +49,15 @@ #define GUIDANCE_H_USE_SPEED_REF TRUE #endif -#define GUIDANCE_H_MODE_KILL 0 -#define GUIDANCE_H_MODE_RATE 1 -#define GUIDANCE_H_MODE_ATTITUDE 2 -#define GUIDANCE_H_MODE_HOVER 3 -#define GUIDANCE_H_MODE_NAV 4 -#define GUIDANCE_H_MODE_RC_DIRECT 5 -#define GUIDANCE_H_MODE_CARE_FREE 6 -#define GUIDANCE_H_MODE_FORWARD 7 +#define GUIDANCE_H_MODE_KILL 0 +#define GUIDANCE_H_MODE_RATE 1 +#define GUIDANCE_H_MODE_ATTITUDE 2 +#define GUIDANCE_H_MODE_HOVER 3 +#define GUIDANCE_H_MODE_NAV 4 +#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_OUTERLOOP 8 extern uint8_t guidance_h_mode; diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index ec8ab3082f..decc718ba0 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -232,6 +232,7 @@ void guidance_v_mode_changed(uint8_t new_mode) case GUIDANCE_V_MODE_RC_CLIMB: case GUIDANCE_V_MODE_CLIMB: guidance_v_zd_sp = 0; + case GUIDANCE_V_MODE_MODULE_OUTERLOOP: case GUIDANCE_V_MODE_NAV: guidance_v_z_sum_err = 0; GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0); @@ -307,6 +308,19 @@ void guidance_v_run(bool_t in_flight) stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; break; + case GUIDANCE_V_MODE_MODULE_OUTERLOOP: + guidance_v_z_sp = -nav_flight_altitude; + guidance_v_zd_sp = 0; + gv_update_ref_from_z_sp(guidance_v_z_sp); + run_hover_loop(in_flight); +#if !NO_RC_THRUST_LIMIT + /* use rc limitation if available */ + if (radio_control.status == RC_OK) { + stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t); + } else +#endif + stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; + break; 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..241757bab9 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h @@ -32,12 +32,13 @@ #include "firmwares/rotorcraft/guidance/guidance_v_ref.h" #include "firmwares/rotorcraft/guidance/guidance_v_adapt.h" -#define GUIDANCE_V_MODE_KILL 0 -#define GUIDANCE_V_MODE_RC_DIRECT 1 -#define GUIDANCE_V_MODE_RC_CLIMB 2 -#define GUIDANCE_V_MODE_CLIMB 3 -#define GUIDANCE_V_MODE_HOVER 4 -#define GUIDANCE_V_MODE_NAV 5 +#define GUIDANCE_V_MODE_KILL 0 +#define GUIDANCE_V_MODE_RC_DIRECT 1 +#define GUIDANCE_V_MODE_RC_CLIMB 2 +#define GUIDANCE_V_MODE_CLIMB 3 +#define GUIDANCE_V_MODE_HOVER 4 +#define GUIDANCE_V_MODE_NAV 5 +#define GUIDANCE_V_MODE_MODULE_OUTERLOOP 6 extern uint8_t guidance_v_mode; diff --git a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c index 2929305a37..6c92c5f7c3 100644 --- a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c +++ b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c @@ -69,9 +69,6 @@ float Error_Vely; unsigned char saturateX = 0, saturateY = 0; unsigned int set_heading; -// TODO FIX -#define AP_MODE_VISION_HOVER 3 - void init_hover_stabilization_onvision() { @@ -95,7 +92,7 @@ void init_hover_stabilization_onvision() void run_hover_stabilization_onvision(void) { - if (autopilot_mode == AP_MODE_VISION_HOVER) { + if (autopilot_mode == AP_MODE_MODULE_OUTERLOOP) { run_opticflow_hover(); } else { Velx_Int = 0; diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c index 0a20b5a550..b7967bfbc8 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -97,6 +97,12 @@ void opticflow_module_run(void) } } +void guidance_module_run(bool_t inflight) +{ + nav_flight_altitude = -1; +} + + ///////////////////////////////////////////////////////////////////////// // COMPUTER VISION THREAD diff --git a/sw/airborne/modules/computer_vision/opticflow_module.h b/sw/airborne/modules/computer_vision/opticflow_module.h index b5e635a1ff..14256bdb5e 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.h +++ b/sw/airborne/modules/computer_vision/opticflow_module.h @@ -29,12 +29,16 @@ #ifndef OPTICFLOW_LAND_H #define OPTICFLOW_LAND_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); +extern void guidance_module_run(bool_t inflight); + // Frame Rate extern float FPS; struct timeval; From 715125ae6d54454d5c48fe09dd54c94feaf461c4 Mon Sep 17 00:00:00 2001 From: dewagter Date: Tue, 13 Jan 2015 20:35:01 +0100 Subject: [PATCH 11/99] [opticflow] activate and set height --- conf/airframes/ardrone2_opticflow_hover.xml | 2 +- sw/airborne/modules/computer_vision/opticflow_module.c | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/conf/airframes/ardrone2_opticflow_hover.xml b/conf/airframes/ardrone2_opticflow_hover.xml index dfb6a69038..acc324ea64 100644 --- a/conf/airframes/ardrone2_opticflow_hover.xml +++ b/conf/airframes/ardrone2_opticflow_hover.xml @@ -216,7 +216,7 @@
- +
diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c index b7967bfbc8..33c32a2c09 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -97,6 +97,8 @@ void opticflow_module_run(void) } } +#include "firmwares/rotorcraft/navigation.h" + void guidance_module_run(bool_t inflight) { nav_flight_altitude = -1; From 348e825a2a8803395bacc8b185ea0f2b34521cac Mon Sep 17 00:00:00 2001 From: dewagter Date: Wed, 14 Jan 2015 09:34:42 +0100 Subject: [PATCH 12/99] [opticflow] remove unused and use global paths --- .../modules/computer_vision/lib/paparazzi.h | 32 ------------------- .../modules/computer_vision/lib/v4l/video.h | 2 +- .../opticflow/opticflow_code.c | 2 +- .../computer_vision/opticflow_module.c | 12 ------- 4 files changed, 2 insertions(+), 46 deletions(-) delete mode 100644 sw/airborne/modules/computer_vision/lib/paparazzi.h diff --git a/sw/airborne/modules/computer_vision/lib/paparazzi.h b/sw/airborne/modules/computer_vision/lib/paparazzi.h deleted file mode 100644 index f5f3627e9c..0000000000 --- a/sw/airborne/modules/computer_vision/lib/paparazzi.h +++ /dev/null @@ -1,32 +0,0 @@ - -//////////////////////////////////////////////// -// Paparazzi communication interface - -#include // pthread_create -#include // gprint - -#include "socket.h" -#include "video_message_structs.h" - -struct gst2ppz_message_struct gst2ppz; -struct ppz2gst_message_struct ppz2gst; - -inline void paparazzi_message_server_start(void); -inline void paparazzi_message_send(void); - -struct UdpSocket *sock; - -inline void paparazzi_message_server_start(void) -{ - sock = udp_socket("192.168.1.1", 2000, 2001, FMS_UNICAST); -} - - -inline void paparazzi_message_send(void) -{ - udp_write(sock, (char *) &gst2ppz, sizeof(gst2ppz)); - int ret = udp_read(sock, (unsigned char *) &ppz2gst, sizeof(ppz2gst)); - printf("read: %d \n", ret); -} - - 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/opticflow_code.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_code.c index 832e8bf864..3d75929b18 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_code.c +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_code.c @@ -35,7 +35,7 @@ // Computer Vision #include "opticflow/optic_flow_ardrone.h" #include "opticflow/fast9/fastRosten.h" -#include "../opticflow_module.h" +#include "modules/computer_vision/opticflow_module.h" // Paparazzi Data #include "subsystems/ins/ins_int.h" diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c index 33c32a2c09..9fda4d884e 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -89,7 +89,6 @@ volatile uint8_t computervision_thread_has_results = 0; void opticflow_module_run(void) { - // Read Latest Vision Module Results if (computervision_thread_has_results) { computervision_thread_has_results = 0; @@ -147,13 +146,6 @@ void *computervision_thread_main(void *data) // Video Grabbing struct img_struct *img_new = video_create_image(&vid); - // Video Resizing -#define DOWNSIZE_FACTOR 1 - struct img_struct small; - small.w = vid.w / DOWNSIZE_FACTOR; - small.h = vid.h / DOWNSIZE_FACTOR; - small.buf = (uint8_t *)malloc(small.w * small.h * 2); - #ifdef DOWNLINK_VIDEO // Video Compression uint8_t *jpegbuf = (uint8_t *)malloc(vid.h * vid.w * 2); @@ -180,12 +172,8 @@ void *computervision_thread_main(void *data) // printf("dt = %d, FPS = %f\n",timestamp, FPS); start_timer(); - // Resize - //resize_uyuv(img_new, &small, DOWNSIZE_FACTOR); - // Run Image Processing my_plugin_run(img_new->buf); - // my_plugin_run(small.buf); #ifdef DOWNLINK_VIDEO // JPEG encode the image: From cacecab21b64e3feda7e058d0a6b19afffb9e7c4 Mon Sep 17 00:00:00 2001 From: dewagter Date: Wed, 14 Jan 2015 09:35:33 +0100 Subject: [PATCH 13/99] [opticflow] Resize function fix: keep color together and also keep working with downsample=1 (copy) --- .../modules/computer_vision/cv/resize.h | 27 ++++++++++++++----- 1 file changed, 20 insertions(+), 7 deletions(-) diff --git a/sw/airborne/modules/computer_vision/cv/resize.h b/sw/airborne/modules/computer_vision/cv/resize.h index 22f06888cf..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 - source += (pixelskip + 1) * 2; - *dest++ = *source++; // U *dest++ = *source++; // V - source += (pixelskip - 1) * 2; + source += pixelskip; + *dest++ = *source++; // Y + source += pixelskip; } - // skip 3 rows - 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; } } From 54ed9ea06e9023991889ba1c4a67c11584269f74 Mon Sep 17 00:00:00 2001 From: dewagter Date: Wed, 14 Jan 2015 14:58:48 +0100 Subject: [PATCH 14/99] [opticflow] fix removal of small --- sw/airborne/modules/computer_vision/opticflow_module.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c index 9fda4d884e..4e62418a8b 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -158,8 +158,8 @@ void *computervision_thread_main(void *data) #endif // First Apply Settings before init - imgWidth = small.w; - imgHeight = small.h; + imgWidth = vid.w; + imgHeight = vid.h; verbose = 2; my_plugin_init(); From cbf1d9ec9f260d602d2df8de07020ff390827370 Mon Sep 17 00:00:00 2001 From: dewagter Date: Wed, 14 Jan 2015 14:59:53 +0100 Subject: [PATCH 15/99] [opticflow] default gains --- .../opticflow/hover_stabilization.c | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c index 6c92c5f7c3..ec643f5205 100644 --- a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c +++ b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c @@ -70,6 +70,36 @@ 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 init_hover_stabilization_onvision() { INT_EULERS_ZERO(cmd_euler); From 1f8fa93cb41b1030f49ae77b20ff7bfcc15f176f Mon Sep 17 00:00:00 2001 From: dewagter Date: Wed, 14 Jan 2015 15:04:50 +0100 Subject: [PATCH 16/99] [opticflow] documentation update --- conf/modules/cv_opticflow.xml | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/conf/modules/cv_opticflow.xml b/conf/modules/cv_opticflow.xml index 146da49cad..8af8ea94d1 100644 --- a/conf/modules/cv_opticflow.xml +++ b/conf/modules/cv_opticflow.xml @@ -13,11 +13,18 @@ + + + + + + + - + From 41095c03f50446a6365fa0568913a4208360e563 Mon Sep 17 00:00:00 2001 From: guidoAI Date: Wed, 14 Jan 2015 06:58:27 -0800 Subject: [PATCH 17/99] removed comments in optic flow code --- .../computer_vision/cv/opticflow/optic_flow_ardrone.c | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.c b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.c index 768758729d..c462f9b512 100644 --- a/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.c +++ b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.c @@ -202,17 +202,6 @@ void getGradientPatch(int *Patch, int *DX, int *DY, int half_window_size) // DY[ix2] = Y2 - Y1; DY[ix2] = (Y2 - Y1) / 2; - /*if(printed < 1 && DX[ix2] > 0) - { - printf("DX = %d, DY = %d\n\r", DX[ix2], DY[ix2]); - printed++; - } - else if(printed == 1 && DX[ix2] < 0) - { - printf("DX = %d, DY = %d\n\r", DX[ix2], DY[ix2]); - printed++; - }*/ - } } From 0ab7a71bef5bd3ae9bf6f5a58ff53164f6aacee2 Mon Sep 17 00:00:00 2001 From: dewagter Date: Thu, 15 Jan 2015 11:41:34 +0100 Subject: [PATCH 18/99] [module-ctrl] GCS name of module control --- sw/ground_segment/cockpit/live.ml | 2 +- sw/ground_segment/tmtc/server_globals.ml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml index 244db7a4d5..c6d1420892 100644 --- a/sw/ground_segment/cockpit/live.ml +++ b/sw/ground_segment/cockpit/live.ml @@ -1265,7 +1265,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; 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"|] From 7a0f034bd893dd95e7a321f41a4d91e0b8cbb8fd Mon Sep 17 00:00:00 2001 From: dewagter Date: Thu, 15 Jan 2015 11:51:16 +0100 Subject: [PATCH 19/99] [module-ctrl] Generic framework to add many new types of controllers in a module --- conf/airframes/ardrone2_opticflow_hover.xml | 2 +- conf/modules/cv_opticflow.xml | 1 - sw/airborne/firmwares/rotorcraft/autopilot.c | 12 +++-- sw/airborne/firmwares/rotorcraft/autopilot.h | 2 +- .../rotorcraft/guidance/guidance_h.c | 26 ++++++---- .../rotorcraft/guidance/guidance_h.h | 18 +++---- .../rotorcraft/guidance/guidance_module.h | 51 +++++++++++++++++++ .../rotorcraft/guidance/guidance_v.c | 25 +++++---- .../rotorcraft/guidance/guidance_v.h | 14 ++--- .../opticflow/hover_stabilization.c | 26 ++++++++-- .../opticflow/hover_stabilization.h | 14 +++++ .../computer_vision/opticflow_module.c | 10 ---- .../computer_vision/opticflow_module.h | 3 ++ 13 files changed, 145 insertions(+), 59 deletions(-) create mode 100644 sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h diff --git a/conf/airframes/ardrone2_opticflow_hover.xml b/conf/airframes/ardrone2_opticflow_hover.xml index acc324ea64..6cdc37a5dd 100644 --- a/conf/airframes/ardrone2_opticflow_hover.xml +++ b/conf/airframes/ardrone2_opticflow_hover.xml @@ -216,7 +216,7 @@
- +
diff --git a/conf/modules/cv_opticflow.xml b/conf/modules/cv_opticflow.xml index 8af8ea94d1..5d2fb518a0 100644 --- a/conf/modules/cv_opticflow.xml +++ b/conf/modules/cv_opticflow.xml @@ -44,7 +44,6 @@ - diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index cae56817ba..3ce4b1846a 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -423,8 +423,10 @@ 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_OUTERLOOP: - guidance_h_mode_changed(GUIDANCE_H_MODE_MODULE_OUTERLOOP); + case AP_MODE_MODULE: +#ifdef GUIDANCE_H_MODE_MODULE_SETTING + guidance_h_mode_changed(GUIDANCE_H_MODE_MODULE_SETTING); +#endif break; default: break; @@ -467,8 +469,10 @@ 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_OUTERLOOP: - guidance_v_mode_changed(GUIDANCE_V_MODE_MODULE_OUTERLOOP); + case AP_MODE_MODULE: +#ifdef GUIDANCE_V_MODE_MODULE_SETTING + guidance_v_mode_changed(GUIDANCE_V_MODE_MODULE); +#endif break; default: break; diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.h b/sw/airborne/firmwares/rotorcraft/autopilot.h index cad980c508..8ee77b2aff 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot.h @@ -50,7 +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_OUTERLOOP 17 +#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 4b56a2f7c2..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,7 +253,12 @@ void guidance_h_mode_changed(uint8_t new_mode) stabilization_attitude_enter(); break; - case GUIDANCE_H_MODE_MODULE_OUTERLOOP: +#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 @@ -305,7 +311,12 @@ void guidance_h_read_rc(bool_t in_flight) #endif break; - case GUIDANCE_H_MODE_MODULE_OUTERLOOP: +#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); @@ -388,15 +399,12 @@ void guidance_h_run(bool_t in_flight) } stabilization_attitude_run(in_flight); break; - case GUIDANCE_H_MODE_MODULE_OUTERLOOP: - if (!in_flight) { - guidance_h_nav_enter(); -#if USE_MODULE_OUTERLOOP==1 - guidance_module_run(in_flight); -#endif - } +#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 c9cd7ff394..574ea4444e 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h @@ -49,15 +49,15 @@ #define GUIDANCE_H_USE_SPEED_REF TRUE #endif -#define GUIDANCE_H_MODE_KILL 0 -#define GUIDANCE_H_MODE_RATE 1 -#define GUIDANCE_H_MODE_ATTITUDE 2 -#define GUIDANCE_H_MODE_HOVER 3 -#define GUIDANCE_H_MODE_NAV 4 -#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_OUTERLOOP 8 +#define GUIDANCE_H_MODE_KILL 0 +#define GUIDANCE_H_MODE_RATE 1 +#define GUIDANCE_H_MODE_ATTITUDE 2 +#define GUIDANCE_H_MODE_HOVER 3 +#define GUIDANCE_H_MODE_NAV 4 +#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..debdbf2005 --- /dev/null +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h @@ -0,0 +1,51 @@ +/* + * Copyright (C) 2008-2009 Antoine Drouin + * + * 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. + * + * 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" + +/** + * + * Horizontal loop default is ATTITUDE + * e.g.: #define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_ATTITUDE + * + * Implement own Horizontal loops when GUIDANCE_H_MODE_MODULE_SETTING is set to GUIDANCE_H_MODE_MODULE + * 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); + * + * Vertical loop default is ATTITUDE + * e.g.: #define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_RC_DIRECT + * + * Implement own Vertical loops when GUIDANCE_V_MODE_MODULE_SETTING is set to GUIDANCE_V_MODE_MODULE + * extern void guidance_v_module_enter(void); + * extern void guidance_v_module_run(bool_t in_flight); + */ + +#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 decc718ba0..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" @@ -232,12 +233,17 @@ void guidance_v_mode_changed(uint8_t new_mode) case GUIDANCE_V_MODE_RC_CLIMB: case GUIDANCE_V_MODE_CLIMB: guidance_v_zd_sp = 0; - case GUIDANCE_V_MODE_MODULE_OUTERLOOP: case GUIDANCE_V_MODE_NAV: guidance_v_z_sum_err = 0; 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; @@ -308,19 +314,12 @@ void guidance_v_run(bool_t in_flight) stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; break; - case GUIDANCE_V_MODE_MODULE_OUTERLOOP: - guidance_v_z_sp = -nav_flight_altitude; - guidance_v_zd_sp = 0; - gv_update_ref_from_z_sp(guidance_v_z_sp); - run_hover_loop(in_flight); -#if !NO_RC_THRUST_LIMIT - /* use rc limitation if available */ - if (radio_control.status == RC_OK) { - stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t); - } else -#endif - stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; +#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 241757bab9..49053be7fd 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h @@ -32,13 +32,13 @@ #include "firmwares/rotorcraft/guidance/guidance_v_ref.h" #include "firmwares/rotorcraft/guidance/guidance_v_adapt.h" -#define GUIDANCE_V_MODE_KILL 0 -#define GUIDANCE_V_MODE_RC_DIRECT 1 -#define GUIDANCE_V_MODE_RC_CLIMB 2 -#define GUIDANCE_V_MODE_CLIMB 3 -#define GUIDANCE_V_MODE_HOVER 4 -#define GUIDANCE_V_MODE_NAV 5 -#define GUIDANCE_V_MODE_MODULE_OUTERLOOP 6 +#define GUIDANCE_V_MODE_KILL 0 +#define GUIDANCE_V_MODE_RC_DIRECT 1 +#define GUIDANCE_V_MODE_RC_CLIMB 2 +#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/modules/computer_vision/opticflow/hover_stabilization.c b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c index ec643f5205..983ea022d9 100644 --- a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c +++ b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c @@ -98,6 +98,27 @@ unsigned int set_heading; #define VISION_DESIRED_VY 0. #endif +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() @@ -122,11 +143,8 @@ void init_hover_stabilization_onvision() void run_hover_stabilization_onvision(void) { - if (autopilot_mode == AP_MODE_MODULE_OUTERLOOP) { + if (autopilot_mode == AP_MODE_MODULE) { run_opticflow_hover(); - } else { - Velx_Int = 0; - Vely_Int = 0; } } diff --git a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.h b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.h index a0a69dda9c..6e5ec518cb 100644 --- a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.h +++ b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.h @@ -31,6 +31,20 @@ #include +// 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(void); void run_opticflow_hover(void); diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c index 4e62418a8b..3e9872e2e5 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -30,9 +30,6 @@ // Own header #include "opticflow_module.h" -// Navigate Based On Vision -#include "opticflow/hover_stabilization.h" - // Paparazzi #include "state.h" // for attitude #include "boards/ardrone/navdata.h" // for ultrasound Height @@ -96,13 +93,6 @@ void opticflow_module_run(void) } } -#include "firmwares/rotorcraft/navigation.h" - -void guidance_module_run(bool_t inflight) -{ - nav_flight_altitude = -1; -} - ///////////////////////////////////////////////////////////////////////// // COMPUTER VISION THREAD diff --git a/sw/airborne/modules/computer_vision/opticflow_module.h b/sw/airborne/modules/computer_vision/opticflow_module.h index 14256bdb5e..e75f084bab 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.h +++ b/sw/airborne/modules/computer_vision/opticflow_module.h @@ -31,6 +31,9 @@ #include "std.h" +// Navigate Based On Vision +#include "opticflow/hover_stabilization.h" + // Module functions extern void opticflow_module_init(void); extern void opticflow_module_run(void); From 155722f09d348a2ce39921518cb2747e991e2f1a Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 15 Jan 2015 16:18:23 +0100 Subject: [PATCH 20/99] [modules] some dox fixes --- .../computer_vision/cv/opticflow/optic_flow_ardrone.c | 9 ++++----- .../computer_vision/cv/opticflow/optic_flow_ardrone.h | 9 ++++----- .../computer_vision/opticflow/hover_stabilization.c | 9 ++++----- .../computer_vision/opticflow/hover_stabilization.h | 9 ++++----- .../modules/computer_vision/opticflow/opticflow_code.c | 9 ++++----- .../modules/computer_vision/opticflow/opticflow_code.h | 9 ++++----- sw/airborne/modules/computer_vision/opticflow_module.c | 7 +++---- sw/airborne/modules/computer_vision/opticflow_module.h | 9 ++++----- 8 files changed, 31 insertions(+), 39 deletions(-) diff --git a/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.c b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.c index c462f9b512..0eaf1b1070 100644 --- a/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.c +++ b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.c @@ -18,13 +18,12 @@ * 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 paparazzi/sw/ext/ardrone2_vision/cv/opticflow/optic_flow_ardrone.c +/** + * @file modules/computer_vision/cv/opticflow/optic_flow_ardrone.c * @brief optical-flow based hovering for Parrot AR.Drone 2.0 * * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 diff --git a/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.h b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.h index b8acfa2608..f791e32dfa 100644 --- a/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.h +++ b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.h @@ -14,13 +14,12 @@ * 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 paparazzi/sw/ext/ardrone2_vision/cv/opticflow/optic_flow_gdc.h +/** + * @file modules/computer_vision/cv/opticflow/optic_flow_ardrone.h * @brief optical-flow based hovering for Parrot AR.Drone 2.0 * * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 diff --git a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c index 983ea022d9..70c2795e6f 100644 --- a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c +++ b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c @@ -14,13 +14,12 @@ * 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 paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/hover_stabilization.c +/** + * @file modules/computer_vision/opticflow/hover_stabilization.c * @brief optical-flow based hovering for Parrot AR.Drone 2.0 * * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 diff --git a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.h b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.h index 6e5ec518cb..b0ce81fb8d 100644 --- a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.h +++ b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.h @@ -14,13 +14,12 @@ * 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 paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/hover_stabilization.h +/** + * @file modules/computer_vision/opticflow/hover_stabilization.h * @brief optical-flow based hovering for Parrot AR.Drone 2.0 * * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_code.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_code.c index 3d75929b18..81b5946556 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_code.c +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_code.c @@ -14,13 +14,12 @@ * 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 paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/opticflow_code.c +/** + * @file modules/computer_vision/optic_flow/opticflow_code.c * @brief optical-flow based hovering for Parrot AR.Drone 2.0 * * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_code.h b/sw/airborne/modules/computer_vision/opticflow/opticflow_code.h index f46fcc0d77..c9cf41c083 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_code.h +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_code.h @@ -14,13 +14,12 @@ * 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 paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/opticflow_code.h +/** + * @file modules/computer_vision/optic_flow/opticflow_code.h * @brief optical-flow based hovering for Parrot AR.Drone 2.0 * * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c index 3e9872e2e5..0048a9e25e 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -14,13 +14,12 @@ * 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 paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/opticflow_module.h + * @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 diff --git a/sw/airborne/modules/computer_vision/opticflow_module.h b/sw/airborne/modules/computer_vision/opticflow_module.h index e75f084bab..7965ce43bd 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.h +++ b/sw/airborne/modules/computer_vision/opticflow_module.h @@ -14,13 +14,12 @@ * 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 paparazzi/sw/ext/ardrone2_vision/modules/OpticFlow/opticflow_module.h +/** + * @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 From c330c9515aff1fccf5ab08910c0f783fe03f8a90 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 25 Dec 2014 23:40:07 +0100 Subject: [PATCH 21/99] [papgets] start working on multi AC papgets --- sw/ground_segment/cockpit/live.ml | 1 + sw/ground_segment/cockpit/papgets.ml | 15 ++++++++++++--- sw/lib/ocaml/papget.ml | 6 +++--- sw/lib/ocaml/papget.mli | 1 + 4 files changed, 17 insertions(+), 6 deletions(-) diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml index 244db7a4d5..69fb005bf8 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; + "sender", ac_id; "icon", icon] in Papget_common.dnd_source b#coerce papget; diff --git a/sw/ground_segment/cockpit/papgets.ml b/sw/ground_segment/cockpit/papgets.ml index 120fdc2b52..62797dd84d 100644 --- a/sw/ground_segment/cockpit/papgets.ml +++ b/sw/ground_segment/cockpit/papgets.ml @@ -42,9 +42,11 @@ let papget_listener = fun papget -> try let field = Papget_common.get_property "field" papget in + let sender = Papget_common.get_property "sender" papget in + prerr_endline field; flush stderr; 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 +74,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 = Papget_common.get_property "sender" papget in + new Papget.expression ~extra_functions ~sender expr @@ -125,12 +128,17 @@ let create = fun canvas_group papget -> 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"; + let sender = Papget_common.get_property "sender" papget in + printf "%s\n" sender; flush stdout; Hashtbl.iter (fun ac_id ac -> + printf "%s %s\n" sender ac_id; flush stdout; + if ac_id = sender then begin 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 + end ) Live.aircrafts in @@ -198,13 +206,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 "sender" 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/lib/ocaml/papget.ml b/sw/lib/ocaml/papget.ml index 8968fcb2bf..7a497cc922 100644 --- a/sw/lib/ocaml/papget.ml +++ b/sw/lib/ocaml/papget.ml @@ -83,7 +83,7 @@ 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 @@ -131,8 +131,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." 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 From 15e1da01f0b6c3ba160895ea9f83a6856fcf340f Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Fri, 2 Jan 2015 23:00:44 +0100 Subject: [PATCH 22/99] [papgets] variable settings bind to a single A/C --- sw/ground_segment/cockpit/live.ml | 2 +- sw/ground_segment/cockpit/page_settings.ml | 13 +++---- sw/ground_segment/cockpit/page_settings.mli | 2 +- sw/ground_segment/cockpit/papgets.ml | 38 +++++++++------------ sw/ground_segment/tmtc/settings.ml | 2 +- 5 files changed, 26 insertions(+), 31 deletions(-) diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml index 69fb005bf8..867f057129 100644 --- a/sw/ground_segment/cockpit/live.ml +++ b/sw/ground_segment/cockpit/live.ml @@ -622,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 -> diff --git a/sw/ground_segment/cockpit/page_settings.ml b/sw/ground_segment/cockpit/page_settings.ml index 4077020502..ee040bd14d 100644 --- a/sw/ground_segment/cockpit/page_settings.ml +++ b/sw/ground_segment/cockpit/page_settings.ml @@ -75,7 +75,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" @@ -252,6 +252,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"; + "sender", ac_id; "icon", icon] in Papget_common.dnd_source b#coerce papget; @@ -282,12 +283,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 @@ -303,18 +304,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 62797dd84d..c41f2a8622 100644 --- a/sw/ground_segment/cockpit/papgets.ml +++ b/sw/ground_segment/cockpit/papgets.ml @@ -127,20 +127,14 @@ 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"; let sender = Papget_common.get_property "sender" papget in - printf "%s\n" sender; flush stdout; - Hashtbl.iter - (fun ac_id ac -> - printf "%s %s\n" sender ac_id; flush stdout; - if ac_id = sender then begin - 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 - end - ) - Live.aircrafts + try + let ac = Hashtbl.find Live.aircrafts sender in + 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 sender block_id + with _ -> () in let properties = [ Papget_common.property "block_name" block_name ] @ locked papget in @@ -159,15 +153,15 @@ 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 sender = Papget_common.get_property "sender" papget in + try + let ac = Hashtbl.find Live.aircrafts sender in + match ac.Live.dl_settings_page with + None -> () + | Some settings -> + let var_id = settings#assoc varname in + Live.dl_setting sender var_id value + with _ -> () in let properties = [ Papget_common.property "variable" varname; 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 -> From 2d07633b6c01b36be8a15e8d9ea436b99e713f91 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 15 Jan 2015 23:20:28 +0100 Subject: [PATCH 23/99] [papgets] handle the case when target A/C is unknown usually the case of GCS layout --- sw/ground_segment/cockpit/papgets.ml | 43 +++++++++++++++++----------- sw/lib/ocaml/papget.ml | 14 ++++++--- 2 files changed, 37 insertions(+), 20 deletions(-) diff --git a/sw/ground_segment/cockpit/papgets.ml b/sw/ground_segment/cockpit/papgets.ml index c41f2a8622..ba94ae2e1b 100644 --- a/sw/ground_segment/cockpit/papgets.ml +++ b/sw/ground_segment/cockpit/papgets.ml @@ -42,11 +42,12 @@ let papget_listener = fun papget -> try let field = Papget_common.get_property "field" papget in - let sender = Papget_common.get_property "sender" papget in - prerr_endline field; flush stderr; - match Str.split sep field with - [msg_name; field_name] -> + let sender = try Some (Papget_common.get_property "sender" papget) with _ -> None in + match Str.split sep field, sender with + [msg_name; field_name], Some sender -> (new Papget.message_field ~sender msg_name field_name) + | [msg_name; field_name], None -> + (new Papget.message_field msg_name field_name) | _ -> failwith (sprintf "Unexpected field spec: %s" field) with _ -> failwith (sprintf "field attr expected in '%s" (Xml.to_string papget)) @@ -74,8 +75,10 @@ let extra_functions = let expression_listener = fun papget -> let expr = Papget_common.get_property "expr" papget in let expr = Expr_lexer.parse expr in - let sender = Papget_common.get_property "sender" papget in - new Papget.expression ~extra_functions ~sender expr + let sender = try Some (Papget_common.get_property "sender" papget) with _ -> None in + match sender with + Some sender -> new Papget.expression ~extra_functions ~sender expr + | None -> new Papget.expression ~extra_functions expr @@ -127,14 +130,18 @@ 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 () -> - let sender = Papget_common.get_property "sender" papget in - try - let ac = Hashtbl.find Live.aircrafts sender in + 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 sender block_id - with _ -> () + Live.jump_to_block ac_id block_id + in + let sender = try Some (Papget_common.get_property "sender" 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 @@ -153,15 +160,19 @@ let create = fun canvas_group papget -> and value = float_of_string (Papget_common.get_property "value" papget) in let clicked = fun () -> - let sender = Papget_common.get_property "sender" papget in - try - let ac = Hashtbl.find Live.aircrafts sender in + 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 sender var_id value - with _ -> () + Live.dl_setting ac_id var_id value + in + let sender = try Some (Papget_common.get_property "sender" 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; diff --git a/sw/lib/ocaml/papget.ml b/sw/lib/ocaml/papget.ml index 7a497cc922..49e0a177e6 100644 --- a/sw/lib/ocaml/papget.ml +++ b/sw/lib/ocaml/papget.ml @@ -86,14 +86,17 @@ end 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 = match sender with + Some sender -> new message_field ~sender i f + | None -> new message_field i f + in Hashtbl.add htable (i, f) msg_obj in loop expr; htable @@ -111,7 +114,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]) -> @@ -132,7 +135,10 @@ let eval_expr = fun (extra_functions:(string * (string list -> string)) list) h class expression = fun ?(extra_functions=[]) ?sender expr -> - let h = hash_vars ~sender expr in + let h = match sender with + Some sender -> hash_vars ~sender expr + | None -> hash_vars expr + in object val mutable callbacks = [] val mutable last_value = "0." From 58418be01369d4d28b0821915e8f7a628f7ce342 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 15 Jan 2015 23:28:25 +0100 Subject: [PATCH 24/99] [papgets] change property name to "ac_id" --- sw/ground_segment/cockpit/live.ml | 2 +- sw/ground_segment/cockpit/page_settings.ml | 2 +- sw/ground_segment/cockpit/papgets.ml | 10 +++++----- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml index 867f057129..108c5726c1 100644 --- a/sw/ground_segment/cockpit/live.ml +++ b/sw/ground_segment/cockpit/live.ml @@ -545,7 +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; - "sender", ac_id; + "ac_id", ac_id; "icon", icon] in Papget_common.dnd_source b#coerce papget; diff --git a/sw/ground_segment/cockpit/page_settings.ml b/sw/ground_segment/cockpit/page_settings.ml index ee040bd14d..cdf41272e8 100644 --- a/sw/ground_segment/cockpit/page_settings.ml +++ b/sw/ground_segment/cockpit/page_settings.ml @@ -252,7 +252,7 @@ let one_setting = fun (i:int) (do_change:int -> float -> unit) ac_id packing dl_ let papget = Papget_common.xml "variable_setting" "button" ["variable", varname; "value", ExtXml.attrib x "value"; - "sender", ac_id; + "ac_id", ac_id; "icon", icon] in Papget_common.dnd_source b#coerce papget; diff --git a/sw/ground_segment/cockpit/papgets.ml b/sw/ground_segment/cockpit/papgets.ml index ba94ae2e1b..6967bd54fc 100644 --- a/sw/ground_segment/cockpit/papgets.ml +++ b/sw/ground_segment/cockpit/papgets.ml @@ -42,7 +42,7 @@ let papget_listener = fun papget -> try let field = Papget_common.get_property "field" papget in - let sender = try Some (Papget_common.get_property "sender" papget) with _ -> None in + let sender = try Some (Papget_common.get_property "ac_id" papget) with _ -> None in match Str.split sep field, sender with [msg_name; field_name], Some sender -> (new Papget.message_field ~sender msg_name field_name) @@ -75,7 +75,7 @@ let extra_functions = let expression_listener = fun papget -> let expr = Papget_common.get_property "expr" papget in let expr = Expr_lexer.parse expr in - let sender = try Some (Papget_common.get_property "sender" papget) with _ -> None in + let sender = try Some (Papget_common.get_property "ac_id" papget) with _ -> None in match sender with Some sender -> new Papget.expression ~extra_functions ~sender expr | None -> new Papget.expression ~extra_functions expr @@ -136,7 +136,7 @@ let create = fun canvas_group papget -> 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 "sender" papget) with _ -> None 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 -> @@ -167,7 +167,7 @@ let create = fun canvas_group papget -> let var_id = settings#assoc varname in Live.dl_setting ac_id var_id value in - let sender = try Some (Papget_common.get_property "sender" papget) with _ -> None 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 -> @@ -218,7 +218,7 @@ let dnd_data_received = fun canvas_group _context ~x ~y data ~info ~time -> "x", sprintf "%d" x; "y", sprintf "%d" y ] and props = [ Papget_common.property "field" (sprintf "%s:%s" msg_name field_name); - Papget_common.property "sender" sender; + 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 From 1c232d0eda694855d2f1a9b73d52f6d67da5efbe Mon Sep 17 00:00:00 2001 From: dewagter Date: Thu, 15 Jan 2015 22:19:33 +0100 Subject: [PATCH 25/99] [opticflow] fix missing include --- .../modules/computer_vision/opticflow/hover_stabilization.c | 1 + 1 file changed, 1 insertion(+) diff --git a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c index 70c2795e6f..cb540876dd 100644 --- a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c +++ b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c @@ -34,6 +34,7 @@ // Stabilization //#include "stabilization.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude.h" +#include "firmwares/rotorcraft/guidance/guidance_v.h" #include "autopilot.h" // Downlink From 9e8a343e9d37fc18a18c9050a24e0a883c2fac6b Mon Sep 17 00:00:00 2001 From: dewagter Date: Thu, 15 Jan 2015 22:25:24 +0100 Subject: [PATCH 26/99] [ctrl_module] demo module --- conf/modules/ctrl_module_demo.xml | 28 ++++++++++ sw/airborne/modules/ctrl/ctrl_module_demo.c | 41 ++++++++++++++ sw/airborne/modules/ctrl/ctrl_module_demo.h | 59 +++++++++++++++++++++ 3 files changed, 128 insertions(+) create mode 100644 conf/modules/ctrl_module_demo.xml create mode 100644 sw/airborne/modules/ctrl/ctrl_module_demo.c create mode 100644 sw/airborne/modules/ctrl/ctrl_module_demo.h diff --git a/conf/modules/ctrl_module_demo.xml b/conf/modules/ctrl_module_demo.xml new file mode 100644 index 0000000000..f5b94b2933 --- /dev/null +++ b/conf/modules/ctrl_module_demo.xml @@ -0,0 +1,28 @@ + + + + + + Demo Control Module + + Attitude controller with own code + + + + + + + + + +
+ +
+ + + + + + + + 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..4c27067df5 --- /dev/null +++ b/sw/airborne/modules/ctrl/ctrl_module_demo.c @@ -0,0 +1,41 @@ +/* + * 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 paparazzi/sw/airborne/modules/ctrl/ctrl_module_demo.h + * @brief example empty controller + * + */ + +#include "modules/ctrl/ctrl_module_demo.h" + + +void my_ctrl_init(void) +{ + +} +void my_ctrl_run(void) +{ + +} + + + 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..85c63e8690 --- /dev/null +++ b/sw/airborne/modules/ctrl/ctrl_module_demo.h @@ -0,0 +1,59 @@ +/* + * 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 paparazzi/sw/airborne/modules/ctrl/ctrl_module_demo.h + * @brief example empty controller + * + */ + +#ifndef CTRL_MODULE_DEMO_H_ +#define CTRL_MODULE_DEMO_H_ + +#include + +// Demo Controller Module +extern void my_ctrl_init(void); +extern void my_ctrl_run(void); +// Settings +extern int ctrl_module_demo_gain; + + + + + +// Vertical loop re-uses Alt-hold +#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_MODULE + +// Horizontal mode is a specific controller +#define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_MODULE + +// Implement own Horizontal loops +inline void guidance_h_module_enter(void) { my_ctrl_init();} +inline void guidance_h_module_read_rc(void) {} +inline void guidance_h_module_run(bool_t in_flight) {my_ctrl_run();} + +// Implement own Horizontal loops +inline void guidance_v_module_enter(void){} +inline void guidance_v_module_run(bool_t in_flight){} + + +#endif /* HOVER_STABILIZATION_H_ */ From 95639bca382ac21554a9aced00a0913f102533e8 Mon Sep 17 00:00:00 2001 From: dewagter Date: Thu, 15 Jan 2015 23:39:24 +0100 Subject: [PATCH 27/99] [ctrl_module] example --- conf/modules/ctrl_module_demo.xml | 10 +-- sw/airborne/modules/ctrl/ctrl_module_demo.c | 72 +++++++++++++++++++-- sw/airborne/modules/ctrl/ctrl_module_demo.h | 31 ++++----- 3 files changed, 88 insertions(+), 25 deletions(-) diff --git a/conf/modules/ctrl_module_demo.xml b/conf/modules/ctrl_module_demo.xml index f5b94b2933..9a32233005 100644 --- a/conf/modules/ctrl_module_demo.xml +++ b/conf/modules/ctrl_module_demo.xml @@ -5,13 +5,16 @@ Demo Control Module - Attitude controller with own code + Rate-controller as module sample - + + + + @@ -19,8 +22,7 @@
- - + diff --git a/sw/airborne/modules/ctrl/ctrl_module_demo.c b/sw/airborne/modules/ctrl/ctrl_module_demo.c index 4c27067df5..90183dea11 100644 --- a/sw/airborne/modules/ctrl/ctrl_module_demo.c +++ b/sw/airborne/modules/ctrl/ctrl_module_demo.c @@ -26,16 +26,80 @@ */ #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; -void my_ctrl_init(void) +} 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; } -void my_ctrl_run(void) -{ +// 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 index 85c63e8690..f1500c374d 100644 --- a/sw/airborne/modules/ctrl/ctrl_module_demo.h +++ b/sw/airborne/modules/ctrl/ctrl_module_demo.h @@ -30,30 +30,27 @@ #include -// Demo Controller Module -extern void my_ctrl_init(void); -extern void my_ctrl_run(void); // Settings -extern int ctrl_module_demo_gain; +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; - - - -// Vertical loop re-uses Alt-hold -#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_MODULE - -// Horizontal mode is a specific controller +// Demo with own guidance_h #define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_MODULE -// Implement own Horizontal loops -inline void guidance_h_module_enter(void) { my_ctrl_init();} -inline void guidance_h_module_read_rc(void) {} -inline void guidance_h_module_run(bool_t in_flight) {my_ctrl_run();} +// and own guidance_v +#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_MODULE // Implement own Horizontal loops -inline void guidance_v_module_enter(void){} -inline void guidance_v_module_run(bool_t in_flight){} +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_ */ From 11d941a651cfba2555d05e1e53c71fbf76303ead Mon Sep 17 00:00:00 2001 From: dewagter Date: Thu, 15 Jan 2015 23:45:14 +0100 Subject: [PATCH 28/99] [guidance_module.h] dox --- .../rotorcraft/guidance/guidance_module.h | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h index debdbf2005..0d9399a09f 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h @@ -21,16 +21,6 @@ /** @file firmwares/rotorcraft/guidance/guidance_module.h * Guidance in a module file. - * - * 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" - -/** * * Horizontal loop default is ATTITUDE * e.g.: #define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_ATTITUDE @@ -46,6 +36,15 @@ * Implement own Vertical loops when GUIDANCE_V_MODE_MODULE_SETTING is set to GUIDANCE_V_MODE_MODULE * extern void guidance_v_module_enter(void); * extern 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_ */ From af69cdec67098eb5ceac7e34cbf907f97899b3d7 Mon Sep 17 00:00:00 2001 From: dewagter Date: Thu, 15 Jan 2015 23:50:11 +0100 Subject: [PATCH 29/99] [guidance_module] author --- sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h index 0d9399a09f..831a66f70f 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2008-2009 Antoine Drouin + * Copyright (C) 2015 * * This file is part of paparazzi. * From c3c7676e1d6e011157a6e581c833d8e8dcbe8d4a Mon Sep 17 00:00:00 2001 From: dewagter Date: Thu, 15 Jan 2015 23:54:49 +0100 Subject: [PATCH 30/99] [conf_tests] Test-compile optic flow --- conf/conf_tests.xml | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/conf/conf_tests.xml b/conf/conf_tests.xml index 00cec549da..24bc060814 100644 --- a/conf/conf_tests.xml +++ b/conf/conf_tests.xml @@ -307,6 +307,17 @@ settings_modules="modules/meteo_france_DAQ.xml" gui_color="blue" /> + Date: Fri, 16 Jan 2015 00:20:45 +0100 Subject: [PATCH 31/99] [opticflow] commenting, renames, dox --- conf/modules/cv_opticflow.xml | 4 +- ...{optic_flow_ardrone.c => optic_flow_int.c} | 111 ++---------------- ...{optic_flow_ardrone.h => optic_flow_int.h} | 7 +- .../opticflow/hover_stabilization.c | 2 +- .../{opticflow_code.c => visual_estimator.c} | 6 +- .../{opticflow_code.h => visual_estimator.h} | 2 +- .../computer_vision/opticflow_module.c | 2 +- 7 files changed, 19 insertions(+), 115 deletions(-) rename sw/airborne/modules/computer_vision/cv/opticflow/{optic_flow_ardrone.c => optic_flow_int.c} (78%) rename sw/airborne/modules/computer_vision/cv/opticflow/{optic_flow_ardrone.h => optic_flow_int.h} (88%) rename sw/airborne/modules/computer_vision/opticflow/{opticflow_code.c => visual_estimator.c} (98%) rename sw/airborne/modules/computer_vision/opticflow/{opticflow_code.h => visual_estimator.h} (95%) diff --git a/conf/modules/cv_opticflow.xml b/conf/modules/cv_opticflow.xml index 5d2fb518a0..546b4e61ff 100644 --- a/conf/modules/cv_opticflow.xml +++ b/conf/modules/cv_opticflow.xml @@ -46,9 +46,9 @@ - + - + diff --git a/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.c b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_int.c similarity index 78% rename from sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.c rename to sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_int.c index 0eaf1b1070..b1e385972e 100644 --- a/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.c +++ b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_int.c @@ -1,9 +1,5 @@ /* - * Copyright (C) 2014 Hann Woei Ho - * - * - Initial fixed-point C implementation by G. de Croon - * - Algorithm: Lucas-Kanade by Yves Bouguet - * - Publication: http://robots.stanford.edu/cs223b04/algo_tracking.pdf + * Copyright (C) 2014 * * This file is part of Paparazzi. * @@ -23,18 +19,20 @@ */ /** - * @file modules/computer_vision/cv/opticflow/optic_flow_ardrone.c - * @brief optical-flow based hovering for Parrot AR.Drone 2.0 + * @file modules/computer_vision/cv/opticflow/optic_flow_int.c + * @brief efficient fixed-point optical-flow * - * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 + * - 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_ardrone.h" -#include "../opticflow_module.h" +#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) @@ -83,12 +81,10 @@ void getSubPixel_gray(int *Patch, unsigned char *frame_buf, int center_x, int ce 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; - //int printed, limit; 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; - //printed = 0; limit = 4; for (i = 0; i < window_size; i++) { for (j = 0; j < window_size; j++) { @@ -107,18 +103,9 @@ void getSubPixel_gray(int *Patch, unsigned char *frame_buf, int center_x, int ce x_0 = x_0_or * subpixel_factor; y_0_or = (y / subpixel_factor); y_0 = y_0_or * subpixel_factor; - /*if(printed < limit) - { - printf("x_0_or = %d, y_0_or = %d;\n\r", x_0_or, y_0_or); - printf("x_0 = %d, y_0 = %d\n\r"); - printed++; - }*/ if (x == x_0 && y == y_0) { - // simply copy the pixel: -// ix2 = uint_index(x_0_or, y_0_or); -// Y = ((unsigned int)frame_buf[ix2+1] + (unsigned int)frame_buf[ix2+3]) >> 1; ix2 = y_0_or * IMG_WIDTH + x_0_or; Y = (unsigned int)frame_buf[ix2 + 1]; Patch[ix1] = (int) Y; @@ -130,42 +117,22 @@ void getSubPixel_gray(int *Patch, unsigned char *frame_buf, int center_x, int ce // 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]; -// ix2 = uint_index(x_0_or, y_0_or); -// Y = ((unsigned int)frame_buf[ix2+1] + (unsigned int)frame_buf[ix2+3]) >> 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]; -// ix2 = uint_index((x_0_or+1), y_0_or); -// Y = ((unsigned int)frame_buf[ix2+1] + (unsigned int)frame_buf[ix2+3]) >> 1; - //if(printed < limit) printf("subpixel: TR = %d\n\r", Y); 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]; -// ix2 = uint_index(x_0_or, (y_0_or+1)); -// Y = ((unsigned int)frame_buf[ix2+1] + (unsigned int)frame_buf[ix2+3]) >> 1; - //if(printed < limit) printf("subpixel: BL = %d\n\r", Y); 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]; -// ix2 = uint_index((x_0_or+1), (y_0_or+1)); -// Y = ((unsigned int)frame_buf[ix2+1] + (unsigned int)frame_buf[ix2+3]) >> 1; - //if(printed < limit) printf("subpixel: BR = %d\n\r", Y); Patch[ix1] += alpha_x * alpha_y * ((int) Y); // normalize patch value Patch[ix1] /= (subpixel_factor * subpixel_factor); - - /*if(printed < limit) - { - - printf("alpha_x = %d, alpha_y = %d, x_0 = %d, y_0 = %d, x = %d, y = %d, Patch[ix1] = %d\n\r", alpha_x, alpha_y, x_0, y_0, x, y, Patch[ix1]); - // printed++; - } - */ - } } } @@ -191,14 +158,12 @@ void getGradientPatch(int *Patch, int *DX, int *DY, int half_window_size) Y1 = Patch[ix1]; ix1 = (unsigned int)(y * padded_patch_size + x + 1); Y2 = Patch[ix1]; -// DX[ix2] = Y2 - Y1; 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; DY[ix2] = (Y2 - Y1) / 2; @@ -215,26 +180,11 @@ int getSumPatch(int *Patch, int size) // 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; - //if(sum < threshold && sum > -threshold) - //{ sum += Patch[ix]; // do not check thresholds - //} - /*else - { - if(sum > threshold) - { - sum = threshold; - } - else - { - sum = -threshold; - } - }*/ } } @@ -342,20 +292,16 @@ int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int } for (p = 0; p < n_found_points; p++) { - //printf("*** NEW POINT ***\n\r"); // status: point is not yet lost: status[p] = 1; - //printf("Normal coordinate: (%d,%d)\n\r", p_x[p], p_y[p]); // 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; - //printf("Subpixel coordinate: (%d,%d)\n\r", p_x[p], p_y[p]); // 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)) { -// printf("Outside of ROI, P1[%d,%d]\n\r",p_x[p],p_y[p]); status[p] = 0; } @@ -380,18 +326,13 @@ int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int if (error == NO_MEMORY) { return NO_MEMORY; } for (it = 0; it < 4; it++) { - // printf("G[%d] = %d\n\r", it, G[it]); G[it] /= 255; // to keep values in range - // printf("G[%d] = %d\n\r", it, G[it]); } // calculate G's determinant: Det = G[0] * G[3] - G[1] * G[2]; - //printf("Det = %d\n\r", Det); Det = Det / subpixel_factor; // so that the steps will be expressed in subpixel units - //printf("Det = %d\n\r", Det); if (Det < 1) { status[p] = 0; -// printf("irrevertible G\n"); } // (4) iterate over taking steps in the image to minimize the error: @@ -403,14 +344,11 @@ int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int step_y = step_threshold + 1; while (status[p] == 1 && it < max_iterations && (abs(step_x) >= step_threshold || abs(step_y) >= step_threshold)) { - //printf("it = %d, (p_x+v_x,p_y+v_y) = (%d,%d)\n\r", it, p_x[p]+v_x, p_y[p]+v_y); - //printf("it = %d;", it); // 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)) { -// printf("Outside of ROI, P1[%d,%d]\n\r",p_x[p],p_y[p]); status[p] = 0; break; } @@ -427,40 +365,14 @@ int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int 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 - //printf("I = "); - //printIntMatrix(I_neighborhood, patch_size, patch_size); - //printf("J = "); - //printIntMatrix(J_neighborhood, patch_size, patch_size); - //getSubPixel(J_neighborhood, new_image_buf, subpixel_factor * ((p_x[p]+v_x)/subpixel_factor), subpixel_factor * ((p_y[p]+v_y) / subpixel_factor), half_window_size, subpixel_factor); - //printf("J2 = "); - //printIntMatrix(J_neighborhood, patch_size, patch_size); - //printf("figure(); subplot(1,2,1); imshow(I/255); subplot(1,2,2); imshow(J/255);\n\r"); getImageDifference(I_neighborhood, J_neighborhood, ImDiff, patch_size, patch_size); - //printf("ImDiff = "); - //printIntMatrix(ImDiff, patch_size, patch_size); error = calculateError(ImDiff, patch_size, patch_size) / 255; -// if(error > error_threshold) printf("error threshold\n"); if (error > error_threshold && it > max_iterations / 2) { status[p] = 0; -// printf("occlusion\n"); break; } - //printf("error(%d) = %d;\n\r", it+1, error); - // [c] calculate the 'b'-vector - //printf("DX = "); - //printIntMatrix(DX, patch_size, patch_size); multiplyImages(ImDiff, DX, IDDX, patch_size, patch_size); - //printf("IDDX = "); - //printIntMatrix(IDDX, patch_size, patch_size); - multiplyImages(ImDiff, DY, IDDY, patch_size, patch_size); - //printf("DY = "); - //printIntMatrix(DY, patch_size, patch_size); - //printf("IDDY = "); - //printIntMatrix(IDDY, patch_size, patch_size); - //printf("figure(); subplot(2,3,1); imagesc(ImDiff); subplot(2,3,2); imagesc(DX); subplot(2,3,3); imagesc(DY);"); - //printf("subplot(2,3,4); imagesc(IDDY); subplot(2,3,5); imagesc(IDDX);\n\r"); - // division by 255 to keep values in range: 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); @@ -469,17 +381,10 @@ int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int 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 - //printf("step = [%d,%d]; v = [%d,%d];\n\r", step_x, step_y, v_x, v_y); - //printf("pause(0.5);\n\r"); // next iteration it++; - // step_size = abs(step_x); - // step_size += abs(step_y); - //printf("status = %d, it = %d, step_size = %d\n\r", status[p], it, step_size); } // iteration to find the right window in the new image - //printf("figure(); plot(error(1:(it+1)));\n\r"); -// printf("it = %d\n",it); 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; diff --git a/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.h b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_int.h similarity index 88% rename from sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.h rename to sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_int.h index f791e32dfa..d1fbc6180e 100644 --- a/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_ardrone.h +++ b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_int.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2014 Hann Woei Ho + * Copyright (C) 2014 * * This file is part of Paparazzi. * @@ -19,10 +19,9 @@ */ /** - * @file modules/computer_vision/cv/opticflow/optic_flow_ardrone.h - * @brief optical-flow based hovering for Parrot AR.Drone 2.0 + * @file modules/computer_vision/cv/opticflow/optic_flow_int.h + * @brief efficient fixed-point optical-flow * - * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 */ #ifndef OPTIC diff --git a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c index cb540876dd..2a3184c71e 100644 --- a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c +++ b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c @@ -29,7 +29,7 @@ #include "hover_stabilization.h" // Vision Data -#include "opticflow_code.h" +#include "visual_estimator.h" // Stabilization //#include "stabilization.h" diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_code.c b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c similarity index 98% rename from sw/airborne/modules/computer_vision/opticflow/opticflow_code.c rename to sw/airborne/modules/computer_vision/opticflow/visual_estimator.c index 81b5946556..7c2d6ab64e 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_code.c +++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c @@ -19,7 +19,7 @@ */ /** - * @file modules/computer_vision/optic_flow/opticflow_code.c + * @file modules/computer_vision/optic_flow/visual_estimator.c * @brief optical-flow based hovering for Parrot AR.Drone 2.0 * * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 @@ -29,10 +29,10 @@ #include // Own Header -#include "opticflow_code.h" +#include "visual_estimator.h" // Computer Vision -#include "opticflow/optic_flow_ardrone.h" +#include "opticflow/optic_flow_int.h" #include "opticflow/fast9/fastRosten.h" #include "modules/computer_vision/opticflow_module.h" diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_code.h b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h similarity index 95% rename from sw/airborne/modules/computer_vision/opticflow/opticflow_code.h rename to sw/airborne/modules/computer_vision/opticflow/visual_estimator.h index c9cf41c083..84fd89ca31 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_code.h +++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h @@ -19,7 +19,7 @@ */ /** - * @file modules/computer_vision/optic_flow/opticflow_code.h + * @file modules/computer_vision/optic_flow/visual_estimator.h * @brief optical-flow based hovering for Parrot AR.Drone 2.0 * * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c index 0048a9e25e..92700919ed 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -101,7 +101,7 @@ void opticflow_module_run(void) #include "resize.h" // Payload Code -#include "opticflow/opticflow_code.h" +#include "opticflow/visual_estimator.h" // Downlink Video //#define DOWNLINK_VIDEO 1 From a3cdd1f56e8c49860aef3fc1d29049cb8cfe0925 Mon Sep 17 00:00:00 2001 From: dewagter Date: Fri, 16 Jan 2015 00:31:26 +0100 Subject: [PATCH 32/99] [opticflow] useless style change --- .../firmwares/rotorcraft/guidance/guidance_h.h | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h index 574ea4444e..626ae4a7e8 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h @@ -49,15 +49,15 @@ #define GUIDANCE_H_USE_SPEED_REF TRUE #endif -#define GUIDANCE_H_MODE_KILL 0 -#define GUIDANCE_H_MODE_RATE 1 -#define GUIDANCE_H_MODE_ATTITUDE 2 -#define GUIDANCE_H_MODE_HOVER 3 -#define GUIDANCE_H_MODE_NAV 4 -#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 +#define GUIDANCE_H_MODE_KILL 0 +#define GUIDANCE_H_MODE_RATE 1 +#define GUIDANCE_H_MODE_ATTITUDE 2 +#define GUIDANCE_H_MODE_HOVER 3 +#define GUIDANCE_H_MODE_NAV 4 +#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; From 76a61ce7206296aab173ec05fc28860188497af5 Mon Sep 17 00:00:00 2001 From: dewagter Date: Fri, 16 Jan 2015 00:40:16 +0100 Subject: [PATCH 33/99] [ctrl_module] dox --- .../rotorcraft/guidance/guidance_module.h | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h index 831a66f70f..362b76608f 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h @@ -22,18 +22,22 @@ /** @file firmwares/rotorcraft/guidance/guidance_module.h * Guidance in a module file. * - * Horizontal loop default is ATTITUDE - * e.g.: #define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_ATTITUDE + * Implement a custom controller in a module + * Re-use desired modes: * - * Implement own Horizontal loops when GUIDANCE_H_MODE_MODULE_SETTING is set to GUIDANCE_H_MODE_MODULE + * 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: * 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); * - * Vertical loop default is ATTITUDE - * e.g.: #define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_RC_DIRECT * - * Implement own Vertical loops when GUIDANCE_V_MODE_MODULE_SETTING is set to GUIDANCE_V_MODE_MODULE + * b) Implement own Vertical loops when GUIDANCE_V_MODE_MODULE_SETTING is set to GUIDANCE_V_MODE_MODULE * extern void guidance_v_module_enter(void); * extern void guidance_v_module_run(bool_t in_flight); * From 87f1ca3e41c5414fc93b553454b8ba5244a62470 Mon Sep 17 00:00:00 2001 From: dewagter Date: Fri, 16 Jan 2015 02:32:53 +0100 Subject: [PATCH 34/99] [build-server] fix tests --- conf/airframes/ardrone2_opticflow_hover.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/conf/airframes/ardrone2_opticflow_hover.xml b/conf/airframes/ardrone2_opticflow_hover.xml index 6cdc37a5dd..fa1f9810e0 100644 --- a/conf/airframes/ardrone2_opticflow_hover.xml +++ b/conf/airframes/ardrone2_opticflow_hover.xml @@ -8,10 +8,10 @@ - + From ed32b8e217841c69738d26a4a2486526a1a500b2 Mon Sep 17 00:00:00 2001 From: dewagter Date: Fri, 16 Jan 2015 11:08:17 +0100 Subject: [PATCH 35/99] [ctrl_module] bugfix --- sw/airborne/firmwares/rotorcraft/autopilot.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index 3ce4b1846a..9795b05802 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -471,7 +471,7 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) break; case AP_MODE_MODULE: #ifdef GUIDANCE_V_MODE_MODULE_SETTING - guidance_v_mode_changed(GUIDANCE_V_MODE_MODULE); + guidance_v_mode_changed(GUIDANCE_V_MODE_MODULE_SETTING); #endif break; default: From 80cc3742e11c99cd5317fd4e05477e50d44fcb3b Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 16 Jan 2015 10:53:34 +0100 Subject: [PATCH 36/99] [modules] more dox fixes --- .../rotorcraft/guidance/guidance_module.h | 21 ++++++++++--------- .../cv/opticflow/optic_flow_int.h | 8 ++++--- .../opticflow/visual_estimator.c | 2 +- .../opticflow/visual_estimator.h | 3 ++- .../computer_vision/opticflow_module.c | 3 --- .../computer_vision/opticflow_module.h | 6 +++--- sw/airborne/modules/ctrl/ctrl_module_demo.c | 9 ++++---- sw/airborne/modules/ctrl/ctrl_module_demo.h | 9 ++++---- 8 files changed, 30 insertions(+), 31 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h index 362b76608f..f0d7cf8aa4 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h @@ -19,29 +19,30 @@ * Boston, MA 02111-1307, USA. */ -/** @file firmwares/rotorcraft/guidance/guidance_module.h - * Guidance in a module file. +/** + * @file firmwares/rotorcraft/guidance/guidance_module.h + * Guidance in a module file. * - * Implement a custom controller in a module + * Implement a custom controller in a module. * Re-use desired modes: * - * e.g.: #define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_HOVER + * 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: - * 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 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 - * extern void guidance_v_module_enter(void); - * extern void guidance_v_module_run(bool_t in_flight); + * - 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 + * If the module implements both V and H mode, take into account that the H is called first and later V * */ 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 index d1fbc6180e..786916ac62 100644 --- a/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_int.h +++ b/sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_int.h @@ -24,8 +24,9 @@ * */ -#ifndef OPTIC -#define OPTIC +#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, @@ -40,4 +41,5 @@ 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 + +#endif /* OPTIC_FLOW_INT_H */ diff --git a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c index 7c2d6ab64e..5c060bc54d 100644 --- a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c +++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c @@ -19,7 +19,7 @@ */ /** - * @file modules/computer_vision/optic_flow/visual_estimator.c + * @file modules/computer_vision/opticflow/visual_estimator.c * @brief optical-flow based hovering for Parrot AR.Drone 2.0 * * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 diff --git a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h index 84fd89ca31..4c656b5ac4 100644 --- a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h +++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h @@ -19,7 +19,7 @@ */ /** - * @file modules/computer_vision/optic_flow/visual_estimator.h + * @file modules/computer_vision/opticflow/visual_estimator.h * @brief optical-flow based hovering for Parrot AR.Drone 2.0 * * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 @@ -45,4 +45,5 @@ void my_plugin_run(unsigned char *frame); // Timer void start_timer_rates(void); long end_timer_rates(void); + #endif diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c index 92700919ed..168d969db3 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -197,6 +197,3 @@ void opticflow_module_stop(void) { computer_vision_thread_command = 0; } - - - diff --git a/sw/airborne/modules/computer_vision/opticflow_module.h b/sw/airborne/modules/computer_vision/opticflow_module.h index 7965ce43bd..78d1ef54b2 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.h +++ b/sw/airborne/modules/computer_vision/opticflow_module.h @@ -25,8 +25,8 @@ * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 */ -#ifndef OPTICFLOW_LAND_H -#define OPTICFLOW_LAND_H +#ifndef OPTICFLOW_MODULE_H +#define OPTICFLOW_MODULE_H #include "std.h" @@ -48,4 +48,4 @@ long time_elapsed(struct timeval *t1, struct timeval *t2); void start_timer(void); long end_timer(void); -#endif /* OPTICFLOW_LAND_H */ +#endif /* OPTICFLOW_MODULE_H */ diff --git a/sw/airborne/modules/ctrl/ctrl_module_demo.c b/sw/airborne/modules/ctrl/ctrl_module_demo.c index 90183dea11..0e3182467e 100644 --- a/sw/airborne/modules/ctrl/ctrl_module_demo.c +++ b/sw/airborne/modules/ctrl/ctrl_module_demo.c @@ -14,13 +14,12 @@ * 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 paparazzi/sw/airborne/modules/ctrl/ctrl_module_demo.h +/** + * @file modules/ctrl/ctrl_module_demo.h * @brief example empty controller * */ diff --git a/sw/airborne/modules/ctrl/ctrl_module_demo.h b/sw/airborne/modules/ctrl/ctrl_module_demo.h index f1500c374d..07a99eef7b 100644 --- a/sw/airborne/modules/ctrl/ctrl_module_demo.h +++ b/sw/airborne/modules/ctrl/ctrl_module_demo.h @@ -14,13 +14,12 @@ * 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 paparazzi/sw/airborne/modules/ctrl/ctrl_module_demo.h +/** + * @file modules/ctrl/ctrl_module_demo.c * @brief example empty controller * */ From 197967f64509dccdf99b748dc1768b5ae260438a Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 16 Jan 2015 12:01:32 +0100 Subject: [PATCH 37/99] [opticflow] use float_quat_vmult to compute V_body --- .../computer_vision/opticflow/visual_estimator.c | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c index 5c060bc54d..ab9df8ba93 100644 --- a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c +++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c @@ -87,8 +87,6 @@ float cam_h, diff_roll, diff_pitch, OFx_trans, OFy_trans; float Velx, Vely; // Compute body velocities -struct FloatVect3 V_Ned; -struct FloatRMat Rmat_Ned2Body; struct FloatVect3 V_body; // Called by plugin @@ -137,13 +135,9 @@ void my_plugin_run(unsigned char *frame) // *********************************************************************************************************************** // Compute body velocities from ENU - V_Ned.x = stateGetSpeedNed_f()->x; - V_Ned.y = stateGetSpeedNed_f()->y; - V_Ned.z = stateGetSpeedNed_f()->z; - - struct FloatQuat *BodyQuaternions = stateGetNedToBodyQuat_f(); - FLOAT_RMAT_OF_QUAT(Rmat_Ned2Body, *BodyQuaternions); - RMAT_VECT3_MUL(V_body, Rmat_Ned2Body, V_Ned); + struct FloatVect3 *vel_ned = (struct FloatVect3*)stateGetSpeedNed_f(); + struct FloatQuat *q_n2b = stateGetNedToBodyQuat_f(); + float_quat_vmult(&V_body, q_n2b, vel_ned); // *********************************************************************************************************************** // Corner detection From ac9f5f0968a2c85209e7226794400bf3099b56e4 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 16 Jan 2015 13:21:01 +0100 Subject: [PATCH 38/99] [opticflow] visual_estimator: remove some stupid global vars --- .../opticflow/visual_estimator.c | 7 +++--- .../opticflow/visual_estimator.h | 23 ++++++++----------- .../computer_vision/opticflow_module.c | 5 +--- 3 files changed, 14 insertions(+), 21 deletions(-) diff --git a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c index ab9df8ba93..742fed798a 100644 --- a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c +++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c @@ -46,9 +46,8 @@ // Timer #include -// Settable by plugin +// Image size set at init unsigned int imgWidth, imgHeight; -unsigned int verbose = 0; // Local variables unsigned char *prev_frame, *gray_frame, *prev_gray_frame; @@ -90,9 +89,11 @@ float Velx, Vely; struct FloatVect3 V_body; // Called by plugin -void my_plugin_init(void) +void my_plugin_init(unsigned int w, unsigned int h) { // Initialize variables + imgWidth = w; + imgHeight = h; gray_frame = (unsigned char *) calloc(imgWidth * imgHeight, sizeof(unsigned char)); prev_frame = (unsigned char *) calloc(imgWidth * imgHeight * 2, sizeof(unsigned char)); prev_gray_frame = (unsigned char *) calloc(imgWidth * imgHeight, sizeof(unsigned char)); diff --git a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h index 4c656b5ac4..64cc7240e4 100644 --- a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h +++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h @@ -25,25 +25,20 @@ * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 */ -#ifndef _OPT_FL_LAND_H -#define _OPT_FL_LAND_H - -// Settable by pluging -extern unsigned int imgWidth, imgHeight; -extern unsigned int verbose; +#ifndef VISUAL_ESTIMATOR_H +#define VISUAL_ESTIMATOR_H // Variables used by the controller extern float Velx, Vely; -extern int count; extern int flow_count; extern struct FloatVect3 V_body; -// Called by plugin -void my_plugin_init(void); +/** + * Initialize visual estimator. + * @param w image width + * @param h image height + */ +void my_plugin_init(unsigned int w, unsigned int h); void my_plugin_run(unsigned char *frame); -// Timer -void start_timer_rates(void); -long end_timer_rates(void); - -#endif +#endif /* VISUAL_ESTIMATOR_H */ diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c index 168d969db3..5ff7b36862 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -147,10 +147,7 @@ void *computervision_thread_main(void *data) #endif // First Apply Settings before init - imgWidth = vid.w; - imgHeight = vid.h; - verbose = 2; - my_plugin_init(); + my_plugin_init(vid.w, vid.h); while (computer_vision_thread_command > 0) { video_grab_image(&vid, img_new); From 2d6f3994f219891fd0478422bdea39ee626eca51 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Sat, 17 Jan 2015 14:29:27 +0100 Subject: [PATCH 39/99] [papgets] save ac_id in layout if needed --- sw/ground_segment/cockpit/papgets.ml | 9 +++++++-- sw/lib/ocaml/papget.ml | 6 ++++-- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/sw/ground_segment/cockpit/papgets.ml b/sw/ground_segment/cockpit/papgets.ml index 6967bd54fc..b887083eef 100644 --- a/sw/ground_segment/cockpit/papgets.ml +++ b/sw/ground_segment/cockpit/papgets.ml @@ -106,6 +106,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" @@ -144,7 +149,7 @@ let create = fun canvas_group papget -> 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 @@ -177,7 +182,7 @@ let create = fun canvas_group papget -> 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 diff --git a/sw/lib/ocaml/papget.ml b/sw/lib/ocaml/papget.ml index 49e0a177e6..2e79863ddb 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 @@ -148,7 +149,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" From 5bd0e0a725c83c16f6c276c7b55009cb7996bc99 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 19 Jan 2015 10:26:02 +0100 Subject: [PATCH 40/99] [papgets] improve readability --- sw/ground_segment/cockpit/papgets.ml | 12 ++++-------- sw/lib/ocaml/papget.ml | 10 ++-------- 2 files changed, 6 insertions(+), 16 deletions(-) diff --git a/sw/ground_segment/cockpit/papgets.ml b/sw/ground_segment/cockpit/papgets.ml index b887083eef..18da2c3ff6 100644 --- a/sw/ground_segment/cockpit/papgets.ml +++ b/sw/ground_segment/cockpit/papgets.ml @@ -43,11 +43,9 @@ let papget_listener = 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, sender with - [msg_name; field_name], Some sender -> - (new Papget.message_field ~sender msg_name field_name) - | [msg_name; field_name], None -> - (new Papget.message_field msg_name field_name) + match Str.split sep field with + [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)) @@ -76,9 +74,7 @@ let expression_listener = fun papget -> let expr = Papget_common.get_property "expr" papget in let expr = Expr_lexer.parse expr in let sender = try Some (Papget_common.get_property "ac_id" papget) with _ -> None in - match sender with - Some sender -> new Papget.expression ~extra_functions ~sender expr - | None -> new Papget.expression ~extra_functions expr + new Papget.expression ~extra_functions ?sender expr diff --git a/sw/lib/ocaml/papget.ml b/sw/lib/ocaml/papget.ml index 2e79863ddb..93b45bcac9 100644 --- a/sw/lib/ocaml/papget.ml +++ b/sw/lib/ocaml/papget.ml @@ -94,10 +94,7 @@ let hash_vars = fun ?sender expr -> | 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 = match sender with - Some sender -> new message_field ~sender i f - | None -> 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 @@ -136,10 +133,7 @@ let eval_expr = fun (extra_functions:(string * (string list -> string)) list) h class expression = fun ?(extra_functions=[]) ?sender expr -> - let h = match sender with - Some sender -> hash_vars ~sender expr - | None -> hash_vars expr - in + let h = hash_vars ?sender expr in object val mutable callbacks = [] val mutable last_value = "0." From ea6bc1d901be119cd9c35796de45828ee8f625a3 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 19 Jan 2015 17:22:09 +0100 Subject: [PATCH 41/99] [papgets] use WIDTH_PIXELS to avoid rescaling --- sw/lib/ocaml/papget_renderer.ml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 From ccf695a9b60f4be0c51709495c5d1aa1db7880fb Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 19 Jan 2015 18:19:33 +0100 Subject: [PATCH 42/99] [papgets] display A/C id in Papget Editor's box title --- sw/lib/ocaml/papget.ml | 3 +++ sw/lib/ocaml/widgets.glade | 3 ++- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/sw/lib/ocaml/papget.ml b/sw/lib/ocaml/papget.ml index 93b45bcac9..9e733e3720 100644 --- a/sw/lib/ocaml/papget.ml +++ b/sw/lib/ocaml/papget.ml @@ -243,6 +243,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/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 From 7763e4d498ffb134b16a4d5fb7e45d3d65799934 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 19 Jan 2015 22:35:44 +0100 Subject: [PATCH 43/99] [modules] multi follow uses state interface for ltp_def --- sw/airborne/modules/multi/follow.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) 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 From f230470a536666883f8b8e7f161eb38f427609f3 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 21 Jan 2015 14:14:31 +0100 Subject: [PATCH 44/99] [papgets] ask if ac_id should be saved --- sw/ground_segment/cockpit/gcs.ml | 8 +++++++- sw/ground_segment/cockpit/papgets.ml | 11 +++++++++-- sw/ground_segment/cockpit/papgets.mli | 2 +- 3 files changed, 17 insertions(+), 4 deletions(-) diff --git a/sw/ground_segment/cockpit/gcs.ml b/sw/ground_segment/cockpit/gcs.ml index 0ca6468b29..924c9770e3 100644 --- a/sw/ground_segment/cockpit/gcs.ml +++ b/sw/ground_segment/cockpit/gcs.ml @@ -693,7 +693,13 @@ 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 = + 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 + 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 diff --git a/sw/ground_segment/cockpit/papgets.ml b/sw/ground_segment/cockpit/papgets.ml index 18da2c3ff6..cd0ce6f980 100644 --- a/sw/ground_segment/cockpit/papgets.ml +++ b/sw/ground_segment/cockpit/papgets.ml @@ -25,13 +25,20 @@ 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 diff --git a/sw/ground_segment/cockpit/papgets.mli b/sw/ground_segment/cockpit/papgets.mli index 5afbaa143e..6f567cc693 100644 --- a/sw/ground_segment/cockpit/papgets.mli +++ b/sw/ground_segment/cockpit/papgets.mli @@ -22,7 +22,7 @@ * *) -val dump_store : unit -> Xml.xml list +val dump_store : bool -> Xml.xml list val create : #GnoCanvas.group -> Xml.xml -> unit val dnd_data_received : #GnoCanvas.group -> From 8fff787ee6f0530e3b90896fa99b905334ccbfb2 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 21 Jan 2015 20:42:34 +0100 Subject: [PATCH 45/99] [papgets] open question box only if the GCS has papgets --- sw/ground_segment/cockpit/gcs.ml | 8 +++++--- sw/ground_segment/cockpit/papgets.ml | 3 +++ sw/ground_segment/cockpit/papgets.mli | 1 + 3 files changed, 9 insertions(+), 3 deletions(-) diff --git a/sw/ground_segment/cockpit/gcs.ml b/sw/ground_segment/cockpit/gcs.ml index 924c9770e3..41c526a667 100644 --- a/sw/ground_segment/cockpit/gcs.ml +++ b/sw/ground_segment/cockpit/gcs.ml @@ -695,9 +695,11 @@ let () = let save_layout = fun () -> (* Ask if ac_id parameters from papgets should be saved *) let save_acid = - 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 + 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 diff --git a/sw/ground_segment/cockpit/papgets.ml b/sw/ground_segment/cockpit/papgets.ml index cd0ce6f980..7ab9945deb 100644 --- a/sw/ground_segment/cockpit/papgets.ml +++ b/sw/ground_segment/cockpit/papgets.ml @@ -44,6 +44,9 @@ let dump_store = fun save_id -> 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 -> diff --git a/sw/ground_segment/cockpit/papgets.mli b/sw/ground_segment/cockpit/papgets.mli index 6f567cc693..9f64c18590 100644 --- a/sw/ground_segment/cockpit/papgets.mli +++ b/sw/ground_segment/cockpit/papgets.mli @@ -23,6 +23,7 @@ *) 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 -> From e72bcd0ae96cf0fcaeb62214aae65b1271730815 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 21 Jan 2015 22:09:39 +0100 Subject: [PATCH 46/99] [papgets] save layout DTD header --- sw/ground_segment/cockpit/gcs.ml | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/sw/ground_segment/cockpit/gcs.ml b/sw/ground_segment/cockpit/gcs.ml index 41c526a667..f31b8d5d10 100644 --- a/sw/ground_segment/cockpit/gcs.ml +++ b/sw/ground_segment/cockpit/gcs.ml @@ -571,6 +571,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 +592,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 () From 70d09527b443f81b572798f21511530c9c88c190 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Wed, 21 Jan 2015 23:32:41 +0100 Subject: [PATCH 47/99] [papgets] delete a papget if placed out of the window on left or top side --- sw/lib/ocaml/papget.ml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/sw/lib/ocaml/papget.ml b/sw/lib/ocaml/papget.ml index 9e733e3720..8236a7b1a7 100644 --- a/sw/lib/ocaml/papget.ml +++ b/sw/lib/ocaml/papget.ml @@ -231,8 +231,13 @@ object (self) | `BUTTON_RELEASE ev -> if GdkEvent.Button.button ev = 1 then begin item#ungrab (GdkEvent.Button.time ev); + let bounds = item#get_bounds in if not motion then begin self#edit () + end + else if (bounds.(0) < 0. && bounds.(2) < 0.) || (bounds.(1) < 0. && bounds.(3) < 0.) then begin + (* delete an item if placed out of the window on the left or top side *) + deleted <- true end; motion <- false end; From 5582ee7fab713ab3a04b36ac5a10f5711bb86243 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 22 Jan 2015 10:40:49 +0100 Subject: [PATCH 48/99] [conf] shared current_sensor subsystem --- .../subsystems/{fixedwing => shared}/current_sensor.makefile | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename conf/firmwares/subsystems/{fixedwing => shared}/current_sensor.makefile (100%) 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 From 3c2cfce3912d1132c41ecb5477d7f8acd50ac97e Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 22 Jan 2015 18:53:16 +0100 Subject: [PATCH 49/99] [papgets] finaly found the window size Papgets are (really) destroyed when placed outside the window --- sw/lib/ocaml/papget.ml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/sw/lib/ocaml/papget.ml b/sw/lib/ocaml/papget.ml index 8236a7b1a7..8bb305485e 100644 --- a/sw/lib/ocaml/papget.ml +++ b/sw/lib/ocaml/papget.ml @@ -231,12 +231,15 @@ 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 (bounds.(0) < 0. && bounds.(2) < 0.) || (bounds.(1) < 0. && bounds.(3) < 0.) then begin + 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 From a2bb0e90c6221affb218cefa31c52ec459abb8f1 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 22 Jan 2015 19:25:48 +0100 Subject: [PATCH 50/99] [python] cleanup a little --- .../python/messages_app/messagesapp.py | 10 +--- .../python/messages_app/messagesframe.py | 24 +++++--- sw/lib/python/messages_tool.py | 8 ++- sw/lib/python/messages_xml_map.py | 57 +++++++++---------- 4 files changed, 50 insertions(+), 49 deletions(-) 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..c43f3c84fb 100644 --- a/sw/ground_segment/python/messages_app/messagesframe.py +++ b/sw/ground_segment/python/messages_app/messagesframe.py @@ -4,7 +4,6 @@ import sys import os import time import threading -import math PPRZ_HOME = os.getenv("PAPARAZZI_HOME") sys.path.append(PPRZ_HOME + "/sw/lib/python") @@ -17,8 +16,20 @@ DATA_WIDTH = 100 HEIGHT = 800 BORDER = 1 + class MessagesFrame(wx.Frame): def message_recv(self, ac_id, name, values): + """Handle incoming messages + + Callback function for IvyMessagesInterface + + :param ac_id: aircraft id + :type ac_id: int + :param name: message name + :type name: str + :param values: message values + :type values: list + """ 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 @@ -31,11 +42,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 +63,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) @@ -84,7 +94,7 @@ class MessagesFrame(wx.Frame): grid_sizer = wx.FlexGridSizer(len(aircraft.messages[name].field_names), 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 @@ -140,5 +150,5 @@ class MessagesFrame(wx.Frame): def OnClose(self, event): self.timer.cancel() - self.interface.Shutdown() + self.interface.shutdown() self.Destroy() diff --git a/sw/lib/python/messages_tool.py b/sw/lib/python/messages_tool.py index 7eaa067024..ac7bd16fd4 100644 --- a/sw/lib/python/messages_tool.py +++ b/sw/lib/python/messages_tool.py @@ -7,9 +7,10 @@ import time import os import re + class Message: def __init__(self, class_name, name): - messages_xml_map.ParseMessages() + messages_xml_map.parse_messages() self.field_value = [] self.field_names = messages_xml_map.message_dictionary[class_name][name] self.field_controls = [] @@ -17,9 +18,10 @@ class Message: self.last_seen = time.clock() self.name = name + class Aircraft: - def __init__(self, id): - self.ac_id = id + def __init__(self, ac_id): + self.ac_id = ac_id self.messages = {} self.messages_book = None diff --git a/sw/lib/python/messages_xml_map.py b/sw/lib/python/messages_xml_map.py index 9cf660677f..d29cd5b5f6 100755 --- a/sw/lib/python/messages_xml_map.py +++ b/sw/lib/python/messages_xml_map.py @@ -3,43 +3,28 @@ from __future__ import absolute_import, print_function import os -import sys -import getopt -messages_path = '%s/conf/messages.xml' % os.getenv("PAPARAZZI_HOME") +default_messages_file = '%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 +class MessagesNotFound(Exception): + def __init__(self, filename): + self.filename = filename + + def __str__(self): + return "messages file " + repr(self.filename) + " not found" -def ParseMessages(): +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_path) + 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: @@ -53,7 +38,7 @@ def ParseMessages(): message_id = the_message.attrib['id'] else: message_id = the_message.attrib['ID'] - if (message_id[0:2] == "0x"): + if message_id[0:2] == "0x": message_id = int(message_id, 16) else: message_id = int(message_id) @@ -67,12 +52,22 @@ def ParseMessages(): 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']) + 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() + 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() From 6d9d29948382d05a9aae6a5161bb1612e82feeb2 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 22 Jan 2015 19:31:39 +0100 Subject: [PATCH 51/99] [python] ivy_msg_interface --- .../python/messages_app/messagesframe.py | 3 +- sw/lib/python/ivy_msg_interface.py | 59 ++++++++++++++++++ sw/lib/python/messages_tool.py | 60 ------------------- 3 files changed, 61 insertions(+), 61 deletions(-) create mode 100644 sw/lib/python/ivy_msg_interface.py diff --git a/sw/ground_segment/python/messages_app/messagesframe.py b/sw/ground_segment/python/messages_app/messagesframe.py index c43f3c84fb..b63b41b128 100644 --- a/sw/ground_segment/python/messages_app/messagesframe.py +++ b/sw/ground_segment/python/messages_app/messagesframe.py @@ -9,6 +9,7 @@ PPRZ_HOME = os.getenv("PAPARAZZI_HOME") sys.path.append(PPRZ_HOME + "/sw/lib/python") import messages_tool +from ivy_msg_interface import IvyMessagesInterface WIDTH = 450 LABEL_WIDTH = 166 @@ -146,7 +147,7 @@ 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.interface = IvyMessagesInterface(self.message_recv) def OnClose(self, event): self.timer.cancel() diff --git a/sw/lib/python/ivy_msg_interface.py b/sw/lib/python/ivy_msg_interface.py new file mode 100644 index 0000000000..4873d526c4 --- /dev/null +++ b/sw/lib/python/ivy_msg_interface.py @@ -0,0 +1,59 @@ +from __future__ import absolute_import, print_function + +from ivy.std_api import * +import logging +import os +import re + + +class IvyMessagesInterface(): + def __init__(self, callback, init=True, bind_regex="(.*)"): + self.callback = callback + self.ivy_id = 0 + 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, 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) + 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 diff --git a/sw/lib/python/messages_tool.py b/sw/lib/python/messages_tool.py index ac7bd16fd4..a4f8227c45 100644 --- a/sw/lib/python/messages_tool.py +++ b/sw/lib/python/messages_tool.py @@ -24,63 +24,3 @@ class Aircraft: self.ac_id = ac_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() From d4d43aca1c14b32b7e76a889ede301a2c677e80c Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 22 Jan 2015 21:01:05 +0100 Subject: [PATCH 52/99] [python] remove messages_tool.py --- .../python/messages_app/messagesframe.py | 25 +++++++++++++++--- sw/lib/python/messages_tool.py | 26 ------------------- 2 files changed, 21 insertions(+), 30 deletions(-) delete mode 100644 sw/lib/python/messages_tool.py diff --git a/sw/ground_segment/python/messages_app/messagesframe.py b/sw/ground_segment/python/messages_app/messagesframe.py index b63b41b128..0ec6039b87 100644 --- a/sw/ground_segment/python/messages_app/messagesframe.py +++ b/sw/ground_segment/python/messages_app/messagesframe.py @@ -8,8 +8,8 @@ import threading PPRZ_HOME = os.getenv("PAPARAZZI_HOME") sys.path.append(PPRZ_HOME + "/sw/lib/python") -import messages_tool from ivy_msg_interface import IvyMessagesInterface +import messages_xml_map WIDTH = 450 LABEL_WIDTH = 166 @@ -18,6 +18,24 @@ HEIGHT = 800 BORDER = 1 +class Message: + def __init__(self, class_name, name): + messages_xml_map.parse_messages() + 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, ac_id): + self.ac_id = ac_id + self.messages = {} + self.messages_book = None + + class MessagesFrame(wx.Frame): def message_recv(self, ac_id, name, values): """Handle incoming messages @@ -34,7 +52,6 @@ class MessagesFrame(wx.Frame): 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) def find_page(self, book, name): @@ -77,7 +94,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) @@ -90,7 +107,7 @@ class MessagesFrame(wx.Frame): def add_new_message(self, aircraft, name): messages_book = aircraft.messages_book - aircraft.messages[name] = messages_tool.Message("telemetry", name) + aircraft.messages[name] = Message("telemetry", name) field_panel = wx.Panel(messages_book) grid_sizer = wx.FlexGridSizer(len(aircraft.messages[name].field_names), 2) diff --git a/sw/lib/python/messages_tool.py b/sw/lib/python/messages_tool.py deleted file mode 100644 index a4f8227c45..0000000000 --- a/sw/lib/python/messages_tool.py +++ /dev/null @@ -1,26 +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.parse_messages() - 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, ac_id): - self.ac_id = ac_id - self.messages = {} - self.messages_book = None From 6f96e8a490dd19752310edbf7c315c0e3618c863 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 22 Jan 2015 22:54:39 +0100 Subject: [PATCH 53/99] [python] IvyMessagesInterface passes msg_class to callback --- .../python/messages_app/messagesframe.py | 36 +++++++++++-------- sw/lib/python/ivy_msg_interface.py | 34 +++++++++++++----- 2 files changed, 46 insertions(+), 24 deletions(-) diff --git a/sw/ground_segment/python/messages_app/messagesframe.py b/sw/ground_segment/python/messages_app/messagesframe.py index 0ec6039b87..5db50f55c8 100644 --- a/sw/ground_segment/python/messages_app/messagesframe.py +++ b/sw/ground_segment/python/messages_app/messagesframe.py @@ -37,22 +37,27 @@ class Aircraft: class MessagesFrame(wx.Frame): - def message_recv(self, ac_id, name, values): + def message_recv(self, msg_class, msg_name, ac_id, values): """Handle incoming messages Callback function for IvyMessagesInterface + :param msg_class: message classe ("ground" or "telemetry") + :param msg_class: string + :param msg_name: message name + :type msg_name: str :param ac_id: aircraft id :type ac_id: int - :param name: message name - :type name: str :param values: message values :type values: list """ - 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: + # only show messages of the requested class + if msg_class != self.msg_class: + return + if ac_id in self.aircrafts and msg_name in self.aircrafts[ac_id].messages: + if time.time() - self.aircrafts[ac_id].messages[msg_name].last_seen < 0.2: return - wx.CallAfter(self.gui_update, ac_id, name, values) + wx.CallAfter(self.gui_update, msg_class, msg_name, ac_id, values) def find_page(self, book, name): if book.GetPageCount() < 1: @@ -105,9 +110,9 @@ 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] = 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) @@ -137,22 +142,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, values): 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]) + aircraft.messages[msg_name].field_controls[index].SetLabel(values[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) @@ -164,6 +169,7 @@ class MessagesFrame(wx.Frame): sizer.Layout() self.timer = threading.Timer(0.1, self.update_leds) self.timer.start() + self.msg_class = msg_class self.interface = IvyMessagesInterface(self.message_recv) def OnClose(self, event): diff --git a/sw/lib/python/ivy_msg_interface.py b/sw/lib/python/ivy_msg_interface.py index 4873d526c4..7e5807e15d 100644 --- a/sw/lib/python/ivy_msg_interface.py +++ b/sw/lib/python/ivy_msg_interface.py @@ -3,13 +3,15 @@ from __future__ import absolute_import, print_function from ivy.std_api import * import logging import os +import sys import re class IvyMessagesInterface(): - def __init__(self, callback, init=True, bind_regex="(.*)"): + def __init__(self, callback, init=True, verbose=True, bind_regex="(.*)"): self.callback = callback self.ivy_id = 0 + self.verbose = verbose self.init_ivy(init, bind_regex) def stop(self): @@ -48,12 +50,26 @@ class IvyMessagesInterface(): data += s.split(' ') else: data.append(s) - try: - ac_id = int(data[0]) - name = data[1] + # ignore ivy message with less than 3 elements + if len(data) < 3: + return + + if data[0] in ["ground", "ground_dl", "dl"]: + msg_class = data[0] + msg_name = data[1] + ac_id = int(data[2]) + values = list(filter(None, data[3:])) + elif data[0] == "sim": + return + else: + try: + ac_id = int(data[0]) + except ValueError: + if self.verbose: + print("ignoring message %s" % data[1]) + sys.stdout.flush() + return + msg_class = "telemetry" + msg_name = data[1] values = list(filter(None, data[2:])) - self.callback(ac_id, name, values) - except ValueError: - pass - except: - raise + self.callback(msg_class, msg_name, ac_id, values) From 5b820f2fe6dd9bdd383ff703d42c9d381e2fe8ce Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Fri, 23 Jan 2015 15:06:31 +0100 Subject: [PATCH 54/99] [module] rewrite nav_bungee_takeoff shorter, with less trigo and more vectors tested in simulation, to be done in real flight --- conf/modules/nav_bungee_takeoff.xml | 24 +- sw/airborne/modules/nav/nav_bungee_takeoff.c | 283 +++++++++---------- sw/airborne/modules/nav/nav_bungee_takeoff.h | 6 +- 3 files changed, 152 insertions(+), 161 deletions(-) diff --git a/conf/modules/nav_bungee_takeoff.xml b/conf/modules/nav_bungee_takeoff.xml index b701b262cc..cac2538d24 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 speed (defined as BUNGEE_TAKEOFF_SPEED in the airframe file). 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/sw/airborne/modules/nav/nav_bungee_takeoff.c b/sw/airborne/modules/nav/nav_bungee_takeoff.c index 6d0ff21fa7..0a698643d9 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. * @@ -22,198 +22,183 @@ /** * @file modules/nav/nav_bungee_takeoff.c * + * Takeoff functions for bungee takeoff. + * * 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 -*/ +/** + * 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 speed (defined as BUNGEE_TAKEOFF_SPEED in the + * airframe file). + * It is also possible to specify the pitch angle (BUNGEE_TAKEOFF_PITCH) and + * the throttle (BUNGEE_TAKEOFF_THROTTLE, between 0 and 1). + * + * @verbatim + *
+ * + * + * + * + * + * + *
+ * @endverbatim + */ -#ifndef Takeoff_Distance -#define Takeoff_Distance 10 +#ifndef BUNGEE_TAKEOFF_DISTANCE +#define BUNGEE_TAKEOFF_DISTANCE 10.0 #endif -#ifndef Takeoff_Height -#define Takeoff_Height 30 +#ifndef BUNGEE_TAKEOFF_HEIGHT +#define BUNGEE_TAKEOFF_HEIGHT 30.0 #endif -#ifndef Takeoff_Speed -#define Takeoff_Speed 15 +#ifndef BUNGEE_TAKEOFF_SPEED +#define BUNGEE_TAKEOFF_SPEED 15.0 +#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 -#ifndef Takeoff_MinSpeed -#define Takeoff_MinSpeed 5 #endif -enum TakeoffStatus { Launch, Throttle, Finished }; +#ifdef Takeoff_Distance +#warning "Takeoff_Distance depreciated, please use BUNGEE_TAKEOFF_DISTANCE instead" +#define BUNGEE_TAKEOFF_DISTANCE Takeoff_Distance +#endif +#ifdef Takeoff_Height +#warning "Takeoff_Height depreciated, please use BUNGEE_TAKEOFF_HEIGHT instead" +#define BUNGEE_TAKEOFF_HEIGHT Takeoff_Height +#endif +#ifdef Takeoff_Speed +#warning "Takeoff_Speed depreciated, please use BUNGEE_TAKEOFF_SPEED instead" +#define BUNGEE_TAKEOFF_SPEED Takeoff_Speed +#endif +#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 +}; + 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) + && ((*stateGetHorizontalSpeedNorm_f()) > BUNGEE_TAKEOFF_SPEED)) { CTakeoffStatus = Finished; return FALSE; } else { @@ -221,7 +206,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..1db820fe06 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. * @@ -22,6 +22,8 @@ /** * @file modules/nav/nav_bungee_takeoff.h * + * Takeoff functions for bungee takeoff. + * * from OSAM advanced navigation routines * */ @@ -31,7 +33,7 @@ #include "std.h" -extern bool_t nav_bungee_takeoff_setup(uint8_t BungeeWP); +extern bool_t nav_bungee_takeoff_setup(uint8_t bungee_wp); extern bool_t nav_bungee_takeoff_run(void); #endif From be0e08bfc237fb93fe4f282aa9483314f9c4c643 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Fri, 23 Jan 2015 16:45:05 +0100 Subject: [PATCH 55/99] [nav] improve dox for bungee takeoff module --- sw/airborne/modules/nav/nav_bungee_takeoff.c | 68 +++++++++----------- sw/airborne/modules/nav/nav_bungee_takeoff.h | 48 +++++++++++++- 2 files changed, 77 insertions(+), 39 deletions(-) diff --git a/sw/airborne/modules/nav/nav_bungee_takeoff.c b/sw/airborne/modules/nav/nav_bungee_takeoff.c index 0a698643d9..67f0424c2d 100644 --- a/sw/airborne/modules/nav/nav_bungee_takeoff.c +++ b/sw/airborne/modules/nav/nav_bungee_takeoff.c @@ -14,9 +14,8 @@ * 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 + * . */ /** @@ -24,7 +23,36 @@ * * Takeoff functions for bungee takeoff. * - * from OSAM advanced navigation routines + * 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 speed (defined as BUNGEE_TAKEOFF_SPEED in the + * airframe file). + * 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 * */ @@ -38,38 +66,6 @@ #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 (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 speed (defined as BUNGEE_TAKEOFF_SPEED in the - * airframe file). - * It is also possible to specify the pitch angle (BUNGEE_TAKEOFF_PITCH) and - * the throttle (BUNGEE_TAKEOFF_THROTTLE, between 0 and 1). - * - * @verbatim - *
- * - * - * - * - * - * - *
- * @endverbatim - */ #ifndef BUNGEE_TAKEOFF_DISTANCE #define BUNGEE_TAKEOFF_DISTANCE 10.0 diff --git a/sw/airborne/modules/nav/nav_bungee_takeoff.h b/sw/airborne/modules/nav/nav_bungee_takeoff.h index 1db820fe06..f64be893dc 100644 --- a/sw/airborne/modules/nav/nav_bungee_takeoff.h +++ b/sw/airborne/modules/nav/nav_bungee_takeoff.h @@ -14,9 +14,8 @@ * 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 + * . */ /** @@ -24,6 +23,35 @@ * * 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 speed (defined as BUNGEE_TAKEOFF_SPEED in the + * airframe file). + * 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 * */ @@ -33,7 +61,21 @@ #include "std.h" +/** 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 From 8402b824bf4dc90964975101d7f9f97df14b6795 Mon Sep 17 00:00:00 2001 From: Freek van Tienen Date: Fri, 23 Jan 2015 13:22:39 +0100 Subject: [PATCH 56/99] [ms5611] Fix i2c transaction --- sw/airborne/peripherals/ms5611_i2c.c | 2 ++ 1 file changed, 2 insertions(+) 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; From b550be3fe1906dd0042f9323bdf8ae91f3538a5e Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 27 Jan 2015 18:20:43 +0100 Subject: [PATCH 57/99] [nav] change ground speed limit to airspeed in bungee takeoff groundspeed may never to reached with strong wing --- conf/modules/nav_bungee_takeoff.xml | 4 +- sw/airborne/modules/nav/nav_bungee_takeoff.c | 58 ++++++++++++-------- sw/airborne/modules/nav/nav_bungee_takeoff.h | 7 ++- 3 files changed, 41 insertions(+), 28 deletions(-) diff --git a/conf/modules/nav_bungee_takeoff.xml b/conf/modules/nav_bungee_takeoff.xml index cac2538d24..0d5e2e6332 100644 --- a/conf/modules/nav_bungee_takeoff.xml +++ b/conf/modules/nav_bungee_takeoff.xml @@ -8,11 +8,11 @@ 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 speed (defined as BUNGEE_TAKEOFF_SPEED in the airframe file). It is also possible to specify the pitch angle (BUNGEE_TAKEOFF_PITCH) and the throttle (BUNGEE_TAKEOFF_THROTTLE, between 0 and 1). + 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/sw/airborne/modules/nav/nav_bungee_takeoff.c b/sw/airborne/modules/nav/nav_bungee_takeoff.c index 67f0424c2d..f68f8a64e2 100644 --- a/sw/airborne/modules/nav/nav_bungee_takeoff.c +++ b/sw/airborne/modules/nav/nav_bungee_takeoff.c @@ -35,15 +35,16 @@ * 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 speed (defined as BUNGEE_TAKEOFF_SPEED in the - * airframe file). + * 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 *
* - * + * * * * @@ -67,14 +68,39 @@ #include "math/pprz_algebra_float.h" +// Backward compatibility +#ifdef Takeoff_Distance +#warning "Takeoff_Distance depreciated, please use BUNGEE_TAKEOFF_DISTANCE instead" +#define BUNGEE_TAKEOFF_DISTANCE Takeoff_Distance +#endif +#ifdef Takeoff_Height +#warning "Takeoff_Height depreciated, please use BUNGEE_TAKEOFF_HEIGHT instead" +#define BUNGEE_TAKEOFF_HEIGHT Takeoff_Height +#endif +#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 +#ifdef Takeoff_MinSpeed +#warning "Takeoff_MinSpeed depreciated, please use BUNGEE_TAKEOFF_MIN_SPEED instead" +#define BUNGEE_TAKEOFF_MIN_SPEED Takeoff_MinSpeed +#endif + + #ifndef BUNGEE_TAKEOFF_DISTANCE #define BUNGEE_TAKEOFF_DISTANCE 10.0 #endif #ifndef BUNGEE_TAKEOFF_HEIGHT #define BUNGEE_TAKEOFF_HEIGHT 30.0 #endif -#ifndef BUNGEE_TAKEOFF_SPEED -#define BUNGEE_TAKEOFF_SPEED 15.0 +#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 @@ -90,23 +116,6 @@ #endif #endif -#ifdef Takeoff_Distance -#warning "Takeoff_Distance depreciated, please use BUNGEE_TAKEOFF_DISTANCE instead" -#define BUNGEE_TAKEOFF_DISTANCE Takeoff_Distance -#endif -#ifdef Takeoff_Height -#warning "Takeoff_Height depreciated, please use BUNGEE_TAKEOFF_HEIGHT instead" -#define BUNGEE_TAKEOFF_HEIGHT Takeoff_Height -#endif -#ifdef Takeoff_Speed -#warning "Takeoff_Speed depreciated, please use BUNGEE_TAKEOFF_SPEED instead" -#define BUNGEE_TAKEOFF_SPEED Takeoff_Speed -#endif -#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, @@ -194,7 +203,10 @@ bool_t nav_bungee_takeoff_run(void) kill_throttle = 0; if ((stateGetPositionUtm_f()->alt > bungee_point.z + BUNGEE_TAKEOFF_HEIGHT) - && ((*stateGetHorizontalSpeedNorm_f()) > BUNGEE_TAKEOFF_SPEED)) { +#if USE_AIRSPEED + && ((*stateGetAirspeed_f()) > BUNGEE_TAKEOFF_AIRSPEED) +#endif + ) { CTakeoffStatus = Finished; return FALSE; } else { diff --git a/sw/airborne/modules/nav/nav_bungee_takeoff.h b/sw/airborne/modules/nav/nav_bungee_takeoff.h index f64be893dc..d7c9c50b6e 100644 --- a/sw/airborne/modules/nav/nav_bungee_takeoff.h +++ b/sw/airborne/modules/nav/nav_bungee_takeoff.h @@ -35,15 +35,16 @@ * 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 speed (defined as BUNGEE_TAKEOFF_SPEED in the - * airframe file). + * 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 *
* - * + * * * * From 97df5d90fae61f558d940dfce4395c5686489122 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 28 Jan 2015 17:34:37 +0100 Subject: [PATCH 58/99] [python] ivy_msg_interface: pass non-telemetry messages with ac_id 0 --- sw/lib/python/ivy_msg_interface.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/sw/lib/python/ivy_msg_interface.py b/sw/lib/python/ivy_msg_interface.py index 7e5807e15d..62130146ae 100644 --- a/sw/lib/python/ivy_msg_interface.py +++ b/sw/lib/python/ivy_msg_interface.py @@ -54,11 +54,13 @@ class IvyMessagesInterface(): if len(data) < 3: return + # check which message class it is + # pass non-telemetry messages with ac_id 0 if data[0] in ["ground", "ground_dl", "dl"]: msg_class = data[0] msg_name = data[1] - ac_id = int(data[2]) - values = list(filter(None, data[3:])) + ac_id = 0 + values = list(filter(None, data[2:])) elif data[0] == "sim": return else: @@ -66,7 +68,7 @@ class IvyMessagesInterface(): ac_id = int(data[0]) except ValueError: if self.verbose: - print("ignoring message %s" % data[1]) + print("ignoring message " + ' '.join(data)) sys.stdout.flush() return msg_class = "telemetry" From 1c48e173a0847755176255aaf0f8df1e5ce2117c Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 27 Jan 2015 17:39:02 +0100 Subject: [PATCH 59/99] [ocaml] try to live in harmony with the garbage collector --- sw/lib/ocaml/convert.c | 61 +++++++++++++++++++++++++----------------- sw/lib/ocaml/cserial.c | 13 ++++++--- 2 files changed, 45 insertions(+), 29 deletions(-) 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); } From b7ab38bdddcf9898dca8335ef8a7ed7c3b3815ce Mon Sep 17 00:00:00 2001 From: gtoonstra Date: Thu, 22 Jan 2015 12:13:30 -0300 Subject: [PATCH 60/99] [python] add ivy_to_redis.py script --- .../python/ivytoredis/ivy_to_redis.py | 125 ++++++++++++++++++ 1 file changed, 125 insertions(+) create mode 100755 sw/ground_segment/python/ivytoredis/ivy_to_redis.py 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..40aafc595a --- /dev/null +++ b/sw/ground_segment/python/ivytoredis/ivy_to_redis.py @@ -0,0 +1,125 @@ +import redis +import time +import os +import sys +import getopt +import signal +from ivy.std_api import * +import logging +import re + +DEFAULT_SERVER = 'localhost' +DEFAULT_PORT = 6379 + +server = 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 = data[0] + name = data[1] + values = list(filter(None, data[2:])) + self.callback(ac_id, name, values) + except IndexError as ie: + logging.error( "Index error on this line: %s"%( l ) ) + +class Ivy2RedisServer(): + def __init__( self, redishost, redisport ): + self.interface = IvyMessagesInterface( self.message_recv ) + self.r = redis.StrictRedis(host=redishost, port=redisport, db=0) + self.keep_running = True + + def message_recv(self, ac_id, name, values): + key = ac_id + "." + name + self.r.publish( key, values ) + self.r.set( key, values ) + + def run( self ): + while( self.keep_running ): + time.sleep( 0.1 ) + + def stop( self ): + self.keep_running = False + self.interface.Shutdown() + +def Usage(scmd): + lpathitem = scmd.split('/') + fmt = '''Usage: %s [-h | --help] [-s redisserver] [-p redisport] +\t-h | --help print this message +\t-s hostname | --server=redisserver the hostname where redis runs +\t-p PORT | --port=PORT where PORT is the number of the port used by redis +''' + print fmt % lpathitem[-1] + +def GetOptions(): + options = {'server':DEFAULT_SERVER, 'port':DEFAULT_PORT} + try: + optlist, left_args = getopt.getopt(sys.argv[1:],'h:s:p:', ['help', 'server=', 'port=']) + 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 ("-s", "--server"): + options['server'] = [ int(a) ] + elif o in ("-p", "--port"): + options['port'] = a + return options + +def signal_handler(signal, frame): + global server + server.stop() + +def main(): + global server + signal.signal(signal.SIGINT, signal_handler) + options = GetOptions() + server = Ivy2RedisServer( options['server'], options['port'] ) + server.run() + +if __name__ == '__main__': + main() From 6462125789913bfad382bcabb9e5f9de2f321089 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 22 Jan 2015 23:01:17 +0100 Subject: [PATCH 61/99] [python] rewrite ivytoredis --- .../python/ivytoredis/ivy_to_redis.py | 136 +++++------------- 1 file changed, 39 insertions(+), 97 deletions(-) diff --git a/sw/ground_segment/python/ivytoredis/ivy_to_redis.py b/sw/ground_segment/python/ivytoredis/ivy_to_redis.py index 40aafc595a..33afb81975 100755 --- a/sw/ground_segment/python/ivytoredis/ivy_to_redis.py +++ b/sw/ground_segment/python/ivytoredis/ivy_to_redis.py @@ -1,124 +1,66 @@ +#!/usr/bin/env python + +from __future__ import print_function + import redis import time -import os -import sys -import getopt import signal -from ivy.std_api import * -import logging -import re +import argparse +import sys +import os -DEFAULT_SERVER = 'localhost' -DEFAULT_PORT = 6379 +PPRZ_HOME = os.getenv("PAPARAZZI_HOME") +sys.path.append(PPRZ_HOME + "/sw/lib/python") + +from ivy_msg_interface import IvyMessagesInterface server = 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 = data[0] - name = data[1] - values = list(filter(None, data[2:])) - self.callback(ac_id, name, values) - except IndexError as ie: - logging.error( "Index error on this line: %s"%( l ) ) class Ivy2RedisServer(): - def __init__( self, redishost, redisport ): - self.interface = IvyMessagesInterface( self.message_recv ) + 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, name, values): - key = ac_id + "." + name - self.r.publish( key, values ) - self.r.set( key, values ) + def message_recv(self, msg_class, msg_name, ac_id, values): + # 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_class, msg_name, ac_id) + else: + key = "{0}.{1}".format(msg_class, msg_name) + if self.verbose: + print("received message, key=%s, values=%s" % (key, ' '.join(values))) + sys.stdout.flush() + self.r.publish(key, values) + self.r.set(key, values) - def run( self ): - while( self.keep_running ): - time.sleep( 0.1 ) + def run(self): + while self.keep_running: + time.sleep(0.1) - def stop( self ): + def stop(self): self.keep_running = False - self.interface.Shutdown() + self.interface.shutdown() -def Usage(scmd): - lpathitem = scmd.split('/') - fmt = '''Usage: %s [-h | --help] [-s redisserver] [-p redisport] -\t-h | --help print this message -\t-s hostname | --server=redisserver the hostname where redis runs -\t-p PORT | --port=PORT where PORT is the number of the port used by redis -''' - print fmt % lpathitem[-1] - -def GetOptions(): - options = {'server':DEFAULT_SERVER, 'port':DEFAULT_PORT} - try: - optlist, left_args = getopt.getopt(sys.argv[1:],'h:s:p:', ['help', 'server=', 'port=']) - 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 ("-s", "--server"): - options['server'] = [ int(a) ] - elif o in ("-p", "--port"): - options['port'] = a - return options 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) - options = GetOptions() - server = Ivy2RedisServer( options['server'], options['port'] ) server.run() if __name__ == '__main__': From ce4e12d9509b0e6f4de05008e44b34a48a66e7f0 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 28 Jan 2015 18:05:42 +0100 Subject: [PATCH 62/99] [conf] sort aircrafts in conf_tests --- conf/conf_example.xml | 2 +- conf/conf_tests.xml | 24 ++++++++++++------------ 2 files changed, 13 insertions(+), 13 deletions(-) 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" /> + - From bd297c7dbd0e71e03f31a173db39c1db93f4dc9e Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 2 Jan 2015 19:48:43 +0100 Subject: [PATCH 63/99] [navigation] start implementing global waypoints - each waypoint is a struct with multiple representations and flags (e.g. if absolute/global) - factor out waypoints in separate files so it can be shared between firmwares later --- .../subsystems/rotorcraft/navigation.makefile | 1 + conf/flight_plans/rotorcraft_basic.xml | 2 +- sw/airborne/firmwares/rotorcraft/navigation.c | 67 ++---- sw/airborne/firmwares/rotorcraft/navigation.h | 31 +-- sw/airborne/subsystems/navigation/waypoints.c | 191 ++++++++++++++++++ sw/airborne/subsystems/navigation/waypoints.h | 99 +++++++++ sw/tools/generators/gen_flight_plan.ml | 8 + 7 files changed, 330 insertions(+), 69 deletions(-) create mode 100644 sw/airborne/subsystems/navigation/waypoints.c create mode 100644 sw/airborne/subsystems/navigation/waypoints.h 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/flight_plans/rotorcraft_basic.xml b/conf/flight_plans/rotorcraft_basic.xml index 04da89ae02..772967481f 100644 --- a/conf/flight_plans/rotorcraft_basic.xml +++ b/conf/flight_plans/rotorcraft_basic.xml @@ -12,7 +12,7 @@ - + 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/subsystems/navigation/waypoints.c b/sw/airborne/subsystems/navigation/waypoints.c new file mode 100644 index 0000000000..9cebd9f7cc --- /dev/null +++ b/sw/airborne/subsystems/navigation/waypoints.c @@ -0,0 +1,191 @@ +/* + * 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)); + ENU_FLOAT_OF_BFP(waypoints[wp_id].enu_f, waypoints[wp_id].enu_i); + waypoints[wp_id].flags &= ~(1 << WP_FLAG_LLA_I); + waypoints[wp_id].flags |= (1 << WP_FLAG_ENU_I | 1 << WP_FLAG_ENU_F); + } +} + +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)); + ENU_BFP_OF_REAL(waypoints[wp_id].enu_i, waypoints[wp_id].enu_f); + waypoints[wp_id].flags &= ~(1 << WP_FLAG_LLA_I); + waypoints[wp_id].flags |= (1 << WP_FLAG_ENU_I | 1 << WP_FLAG_ENU_F); + } +} + +void nav_move_waypoint_enu_i(uint8_t wp_id, struct EnuCoor_i *new_pos) +{ + if (wp_id < nb_waypoint) { + VECT3_COPY(waypoints[wp_id].enu_i, (*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); + } +} + +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)); + waypoints[wp_id].flags |= (1 << 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; + waypoints[wp_id].flags |= (1 << 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); + } +} + +/** 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)); + ENU_FLOAT_OF_BFP(waypoints[wp_id].enu_f, waypoints[wp_id].enu_i); + waypoints[wp_id].flags |= (1 << WP_FLAG_ENU_I | 1 << 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); + } + } +} diff --git a/sw/airborne/subsystems/navigation/waypoints.h b/sw/airborne/subsystems/navigation/waypoints.h new file mode 100644 index 0000000000..88124d4997 --- /dev/null +++ b/sw/airborne/subsystems/navigation/waypoints.h @@ -0,0 +1,99 @@ +/* + * 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 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); + + +#endif /* WAYPOINTS_H */ 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" "{ \\"; From 023b3cbfb4b6c64b81670ec0dbc8103acfca95ec Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 14 Jan 2015 13:29:28 +0100 Subject: [PATCH 64/99] [navigation] waypoints: also set LLA on ENU update --- sw/airborne/subsystems/navigation/waypoints.c | 15 ++++++++++++++- sw/airborne/subsystems/navigation/waypoints.h | 3 +++ 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/sw/airborne/subsystems/navigation/waypoints.c b/sw/airborne/subsystems/navigation/waypoints.c index 9cebd9f7cc..eb0e5ac899 100644 --- a/sw/airborne/subsystems/navigation/waypoints.c +++ b/sw/airborne/subsystems/navigation/waypoints.c @@ -59,6 +59,7 @@ void nav_set_waypoint_enu_i(uint8_t wp_id, struct EnuCoor_i *enu) ENU_FLOAT_OF_BFP(waypoints[wp_id].enu_f, waypoints[wp_id].enu_i); waypoints[wp_id].flags &= ~(1 << WP_FLAG_LLA_I); waypoints[wp_id].flags |= (1 << WP_FLAG_ENU_I | 1 << WP_FLAG_ENU_F); + nav_globalize_local_wp(wp_id); } } @@ -69,13 +70,14 @@ void nav_set_waypoint_enu_f(uint8_t wp_id, struct EnuCoor_f *enu) ENU_BFP_OF_REAL(waypoints[wp_id].enu_i, waypoints[wp_id].enu_f); waypoints[wp_id].flags &= ~(1 << WP_FLAG_LLA_I); waypoints[wp_id].flags |= (1 << WP_FLAG_ENU_I | 1 << WP_FLAG_ENU_F); + 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) { - VECT3_COPY(waypoints[wp_id].enu_i, (*new_pos)); + 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)); } @@ -93,6 +95,7 @@ void nav_set_waypoint_xy_i(uint8_t wp_id, int32_t x, int32_t 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); } } @@ -163,6 +166,16 @@ void nav_set_waypoint_here_2d(uint8_t wp_id) } } +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); + waypoints[wp_id].flags |= (1 << WP_FLAG_LLA_I); + } +} + /** update local ENU coordinates from its LLA coordinates */ void nav_localize_global_wp(uint8_t wp_id) { diff --git a/sw/airborne/subsystems/navigation/waypoints.h b/sw/airborne/subsystems/navigation/waypoints.h index 88124d4997..279a19bd84 100644 --- a/sw/airborne/subsystems/navigation/waypoints.h +++ b/sw/airborne/subsystems/navigation/waypoints.h @@ -90,6 +90,9 @@ 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 */ From e4819b495ea6d5b3cef663599ec4915eadcda1e1 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 14 Jan 2015 13:35:45 +0100 Subject: [PATCH 65/99] [waypoints] use SetBit and ClearBit macros --- sw/airborne/subsystems/navigation/waypoints.c | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/sw/airborne/subsystems/navigation/waypoints.c b/sw/airborne/subsystems/navigation/waypoints.c index eb0e5ac899..358523b123 100644 --- a/sw/airborne/subsystems/navigation/waypoints.c +++ b/sw/airborne/subsystems/navigation/waypoints.c @@ -56,9 +56,10 @@ 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); - waypoints[wp_id].flags &= ~(1 << WP_FLAG_LLA_I); - waypoints[wp_id].flags |= (1 << WP_FLAG_ENU_I | 1 << WP_FLAG_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); } } @@ -67,9 +68,10 @@ 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); - waypoints[wp_id].flags &= ~(1 << WP_FLAG_LLA_I); - waypoints[wp_id].flags |= (1 << WP_FLAG_ENU_I | 1 << WP_FLAG_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); } } @@ -105,7 +107,7 @@ void nav_set_waypoint_lla(uint8_t wp_id, struct LlaCoor_i *lla) return; } memcpy(&waypoints[wp_id].lla, lla, sizeof(struct LlaCoor_i)); - waypoints[wp_id].flags |= (1 << WP_FLAG_LLA_I); + SetBit(waypoints[wp_id].flags, WP_FLAG_LLA_I); nav_localize_global_wp(wp_id); } @@ -136,7 +138,7 @@ void nav_set_waypoint_latlon(uint8_t wp_id, struct LlaCoor_i *lla) } waypoints[wp_id].lla.lat = lla->lat; waypoints[wp_id].lla.lon = lla->lon; - waypoints[wp_id].flags |= (1 << WP_FLAG_LLA_I); + SetBit(waypoints[wp_id].flags, WP_FLAG_LLA_I); nav_localize_global_wp(wp_id); } @@ -172,7 +174,7 @@ void nav_globalize_local_wp(uint8_t wp_id) 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); - waypoints[wp_id].flags |= (1 << WP_FLAG_LLA_I); + SetBit(waypoints[wp_id].flags, WP_FLAG_LLA_I); } } @@ -187,8 +189,9 @@ void nav_localize_global_wp(uint8_t wp_id) 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); - waypoints[wp_id].flags |= (1 << WP_FLAG_ENU_I | 1 << WP_FLAG_ENU_F); + SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_F); } } From 6b6d57ef04fb96053cb7dd645e0876b806e4af37 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 28 Jan 2015 18:20:46 +0100 Subject: [PATCH 66/99] [waypoints] add nav_get_waypoint_lla --- sw/airborne/subsystems/navigation/waypoints.c | 20 +++++++++++++++++++ sw/airborne/subsystems/navigation/waypoints.h | 8 ++++++++ 2 files changed, 28 insertions(+) diff --git a/sw/airborne/subsystems/navigation/waypoints.c b/sw/airborne/subsystems/navigation/waypoints.c index 358523b123..e2765bc91b 100644 --- a/sw/airborne/subsystems/navigation/waypoints.c +++ b/sw/airborne/subsystems/navigation/waypoints.c @@ -205,3 +205,23 @@ 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 + */ +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 index 279a19bd84..ce06a67848 100644 --- a/sw/airborne/subsystems/navigation/waypoints.h +++ b/sw/airborne/subsystems/navigation/waypoints.h @@ -98,5 +98,13 @@ 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 */ From 5e4f7727e849bedde63315912e16308b966128e2 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 28 Jan 2015 21:02:10 +0100 Subject: [PATCH 67/99] [opticflow] remove unused includes --- .../modules/computer_vision/opticflow/visual_estimator.c | 7 ++++--- sw/airborne/modules/computer_vision/opticflow_module.c | 1 - 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c index 742fed798a..1c9041065e 100644 --- a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c +++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c @@ -34,11 +34,12 @@ // Computer Vision #include "opticflow/optic_flow_int.h" #include "opticflow/fast9/fastRosten.h" + +// for FPS #include "modules/computer_vision/opticflow_module.h" // Paparazzi Data -#include "subsystems/ins/ins_int.h" -#include "subsystems/imu.h" +#include "state.h" // Downlink #include "subsystems/datalink/downlink.h" @@ -274,7 +275,7 @@ void my_plugin_run(unsigned char *frame) OFfilter(&OFx, &OFy, OFx_trans, OFy_trans, flow_count, 1); // Velocity Computation -#ifdef USE_SONAR +#if USE_SONAR cam_h = 1; //ins_impl.sonar_z; #else cam_h = 1; diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c index 5ff7b36862..57d682cbc1 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -31,7 +31,6 @@ // Paparazzi #include "state.h" // for attitude -#include "boards/ardrone/navdata.h" // for ultrasound Height // Threaded computer vision #include From 00d2d3dd3384a661935c9dd700e2c76886e871f3 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 28 Jan 2015 21:56:30 +0100 Subject: [PATCH 68/99] [opticflow] cleanup exported functions --- .../opticflow/hover_stabilization.c | 2 ++ .../opticflow/hover_stabilization.h | 1 - .../computer_vision/opticflow_module.c | 26 ++++++++++++------- .../computer_vision/opticflow_module.h | 9 +------ 4 files changed, 20 insertions(+), 18 deletions(-) diff --git a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c index 2a3184c71e..59bc249f16 100644 --- a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c +++ b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c @@ -98,6 +98,8 @@ unsigned int set_heading; #define VISION_DESIRED_VY 0. #endif +void run_opticflow_hover(void); + void guidance_h_module_enter(void) { // INIT diff --git a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.h b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.h index b0ce81fb8d..92a6baeb4d 100644 --- a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.h +++ b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.h @@ -46,7 +46,6 @@ extern void guidance_h_module_run(bool_t in_flight); void init_hover_stabilization_onvision(void); void run_hover_stabilization_onvision(void); -void run_opticflow_hover(void); extern bool activate_opticflow_hover; extern float vision_desired_vx; diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c index 57d682cbc1..c5bee8237a 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -26,9 +26,11 @@ */ -// Own header #include "opticflow_module.h" +// Navigate Based On Vision, needed to call init/run_hover_stabilization_onvision +#include "opticflow/hover_stabilization.h" + // Paparazzi #include "state.h" // for attitude @@ -37,10 +39,17 @@ // Frame Rate (FPS) #include -float FPS; -volatile long timestamp; + #define USEC_PER_SEC 1000000L -long time_elapsed(struct timeval *t1, struct timeval *t2) + +float FPS; + +// local variables +volatile long timestamp; +struct timeval start_time; +struct timeval end_time; + +static long time_elapsed(struct timeval *t1, struct timeval *t2) { long sec, usec; sec = t2->tv_sec - t1->tv_sec; @@ -52,19 +61,18 @@ long time_elapsed(struct timeval *t1, struct timeval *t2) return sec * USEC_PER_SEC + usec; } -struct timeval start_time; -struct timeval end_time; - -void start_timer() +static void start_timer(void) { gettimeofday(&start_time, NULL); } -long end_timer() + +static long end_timer(void) { gettimeofday(&end_time, NULL); return time_elapsed(&start_time, &end_time); } + void opticflow_module_init(void) { // Immediately start the vision thread when the module initialized diff --git a/sw/airborne/modules/computer_vision/opticflow_module.h b/sw/airborne/modules/computer_vision/opticflow_module.h index 78d1ef54b2..332ae0eca2 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.h +++ b/sw/airborne/modules/computer_vision/opticflow_module.h @@ -30,9 +30,6 @@ #include "std.h" -// Navigate Based On Vision -#include "opticflow/hover_stabilization.h" - // Module functions extern void opticflow_module_init(void); extern void opticflow_module_run(void); @@ -41,11 +38,7 @@ extern void opticflow_module_stop(void); extern void guidance_module_run(bool_t inflight); -// Frame Rate +/// Frame Rate extern float FPS; -struct timeval; -long time_elapsed(struct timeval *t1, struct timeval *t2); -void start_timer(void); -long end_timer(void); #endif /* OPTICFLOW_MODULE_H */ From aa1317b2eda35e83bf0a3659d28721760e1012ee Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 28 Jan 2015 22:08:57 +0100 Subject: [PATCH 69/99] [opticflow] rename my_plugin to opticflow_plugin --- .../modules/computer_vision/opticflow/visual_estimator.c | 4 ++-- .../modules/computer_vision/opticflow/visual_estimator.h | 4 ++-- sw/airborne/modules/computer_vision/opticflow_module.c | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c index 1c9041065e..abcd075606 100644 --- a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c +++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c @@ -90,7 +90,7 @@ float Velx, Vely; struct FloatVect3 V_body; // Called by plugin -void my_plugin_init(unsigned int w, unsigned int h) +void opticflow_plugin_init(unsigned int w, unsigned int h) { // Initialize variables imgWidth = w; @@ -124,7 +124,7 @@ void my_plugin_init(unsigned int w, unsigned int h) Vely = 0.0; } -void my_plugin_run(unsigned char *frame) +void opticflow_plugin_run(unsigned char *frame) { if (old_img_init == 1) { memcpy(prev_frame, frame, imgHeight * imgWidth * 2); diff --git a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h index 64cc7240e4..5526a8676a 100644 --- a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h +++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h @@ -38,7 +38,7 @@ extern struct FloatVect3 V_body; * @param w image width * @param h image height */ -void my_plugin_init(unsigned int w, unsigned int h); -void my_plugin_run(unsigned char *frame); +void opticflow_plugin_init(unsigned int w, unsigned int h); +void opticflow_plugin_run(unsigned char *frame); #endif /* VISUAL_ESTIMATOR_H */ diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c index c5bee8237a..7186c006c5 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -154,7 +154,7 @@ void *computervision_thread_main(void *data) #endif // First Apply Settings before init - my_plugin_init(vid.w, vid.h); + opticflow_plugin_init(vid.w, vid.h); while (computer_vision_thread_command > 0) { video_grab_image(&vid, img_new); @@ -166,7 +166,7 @@ void *computervision_thread_main(void *data) start_timer(); // Run Image Processing - my_plugin_run(img_new->buf); + opticflow_plugin_run(img_new->buf); #ifdef DOWNLINK_VIDEO // JPEG encode the image: From 7eb4a86a887096f304743c87427336c7f120b912 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 28 Jan 2015 22:09:48 +0100 Subject: [PATCH 70/99] [opticflow] remove unused guidance_module_run --- sw/airborne/modules/computer_vision/opticflow_module.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/sw/airborne/modules/computer_vision/opticflow_module.h b/sw/airborne/modules/computer_vision/opticflow_module.h index 332ae0eca2..4f6e394e04 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.h +++ b/sw/airborne/modules/computer_vision/opticflow_module.h @@ -36,8 +36,6 @@ extern void opticflow_module_run(void); extern void opticflow_module_start(void); extern void opticflow_module_stop(void); -extern void guidance_module_run(bool_t inflight); - /// Frame Rate extern float FPS; From 5a7e862e903e32ae4e48bec6b5999631d9a52f0d Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 28 Jan 2015 22:35:02 +0100 Subject: [PATCH 71/99] [opticflow] cosmetics: max line length --- .../opticflow/visual_estimator.c | 33 ++++++++++--------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c index abcd075606..9dd96776d9 100644 --- a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c +++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c @@ -86,7 +86,6 @@ float cam_h, diff_roll, diff_pitch, OFx_trans, OFy_trans; // Lateral Velocity Computation float Velx, Vely; -// Compute body velocities struct FloatVect3 V_body; // Called by plugin @@ -132,23 +131,24 @@ void opticflow_plugin_run(unsigned char *frame) old_img_init = 0; } - // *********************************************************************************************************************** + // ************************************************************************************* // Additional information from other sensors - // *********************************************************************************************************************** + // ************************************************************************************* // 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); - // *********************************************************************************************************************** + // ************************************************************************************* // Corner detection - // *********************************************************************************************************************** + // ************************************************************************************* // FAST corner detection int fast_threshold = 20; xyFAST *pnts_fast; - pnts_fast = fast9_detect((const byte *)prev_gray_frame, imgWidth, imgHeight, imgWidth, fast_threshold, &count); + pnts_fast = fast9_detect((const byte *)prev_gray_frame, imgWidth, imgHeight, imgWidth, + fast_threshold, &count); if (count > MAX_COUNT) { count = MAX_COUNT; } for (int i = 0; i < count; i++) { @@ -193,13 +193,13 @@ void opticflow_plugin_run(unsigned char *frame) count = count_fil; free(labelmin); - // ********************************************************************************************************************** + // ************************************************************************************* // Corner Tracking - // ********************************************************************************************************************** + // ************************************************************************************* CvtYUYV2Gray(gray_frame, frame, imgWidth, imgHeight); - error_opticflow = opticFlowLK(gray_frame, prev_gray_frame, x, y, count_fil, imgWidth, imgHeight, new_x, new_y, status, - 5, 100); + error_opticflow = opticFlowLK(gray_frame, prev_gray_frame, x, y, count_fil, imgWidth, + imgHeight, new_x, new_y, status, 5, 100); flow_count = count_fil; for (int i = count_fil - 1; i >= 0; i--) { @@ -289,17 +289,18 @@ void opticflow_plugin_run(unsigned char *frame) Vely = 0.0; } - // ********************************************************************************************************************** + // ************************************************************************************* // Next Loop Preparation - // ********************************************************************************************************************** + // ************************************************************************************* memcpy(prev_frame, frame, imgHeight * imgWidth * 2); memcpy(prev_gray_frame, gray_frame, imgHeight * imgWidth); - // ********************************************************************************************************************** + // ************************************************************************************* // Downlink Message - // ********************************************************************************************************************** - DOWNLINK_SEND_OF_HOVER(DefaultChannel, DefaultDevice, &FPS, &dx_sum, &dy_sum, &OFx, &OFy, &diff_roll, &diff_pitch, - &Velx, &Vely, &V_body.x, &V_body.y, &cam_h, &count); + // ************************************************************************************* + DOWNLINK_SEND_OF_HOVER(DefaultChannel, DefaultDevice, &FPS, &dx_sum, &dy_sum, &OFx, &OFy, + &diff_roll, &diff_pitch, &Velx, &Vely, &V_body.x, &V_body.y, + &cam_h, &count); } From 067f91c9f5cf20ead9da8f7a976b2a9e34e1014e Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 28 Jan 2015 22:16:39 +0100 Subject: [PATCH 72/99] [opticflow] get AGL via ABI to get the scale when computing velocity from flow --- .../opticflow/visual_estimator.c | 35 ++++++++++++++++--- 1 file changed, 30 insertions(+), 5 deletions(-) diff --git a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c index 9dd96776d9..7b0288a44f 100644 --- a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c +++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c @@ -40,6 +40,7 @@ // Paparazzi Data #include "state.h" +#include "subsystems/abi.h" // Downlink #include "subsystems/datalink/downlink.h" @@ -88,6 +89,25 @@ float Velx, Vely; struct FloatVect3 V_body; +/** height above ground level, from ABI + * Used for scale computation, negative value means invalid. + */ +volatile float estimator_agl; + +/** 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, const float *distance); + +static void agl_cb(uint8_t sender_id __attribute__((unused)), const float *distance) +{ + if (distance > 0) { + estimator_agl = *distance; + } +} + // Called by plugin void opticflow_plugin_init(unsigned int w, unsigned int h) { @@ -121,6 +141,10 @@ void opticflow_plugin_init(unsigned int w, unsigned int h) OFy_trans = 0.0; Velx = 0.0; Vely = 0.0; + + // get AGL from sonar via ABI + estimator_agl = -1.0; + AbiBindMsgAGL(OPTICFLOW_AGL_ID, &agl_ev, agl_cb); } void opticflow_plugin_run(unsigned char *frame) @@ -275,11 +299,12 @@ void opticflow_plugin_run(unsigned char *frame) OFfilter(&OFx, &OFy, OFx_trans, OFy_trans, flow_count, 1); // Velocity Computation -#if USE_SONAR - cam_h = 1; //ins_impl.sonar_z; -#else - cam_h = 1; -#endif + if (estimator_agl < 0) { + cam_h = 1; + } + else { + cam_h = estimator_agl; + } if (flow_count) { Velx = OFy * cam_h * FPS / Fy_ARdrone + 0.05; From 9df34e1ed31ab59658f7735edd685d839de470eb Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Wed, 28 Jan 2015 22:52:36 +0100 Subject: [PATCH 73/99] [opticflow] remove unused USE_ARDRONE_VIDEO define --- conf/modules/cv_opticflow.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/conf/modules/cv_opticflow.xml b/conf/modules/cv_opticflow.xml index 546b4e61ff..0b9cce7ee2 100644 --- a/conf/modules/cv_opticflow.xml +++ b/conf/modules/cv_opticflow.xml @@ -43,7 +43,6 @@ - From 9c877f5624e6189f4491bceffb2533609c780ec4 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 29 Jan 2015 09:43:46 +0100 Subject: [PATCH 74/99] [messages] fix hide/show of notebook with multi A/C --- sw/ground_segment/tmtc/messages.ml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/sw/ground_segment/tmtc/messages.ml b/sw/ground_segment/tmtc/messages.ml index a4dfc95aee..34601f4d3f 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 @@ -197,7 +197,7 @@ let rec one_class = fun (notebook:GPack.notebook) (help_label:GObj.widget) (wind (** 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 From ee596bc333d1cffdf605fbfa87646c08064af45a Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 29 Jan 2015 10:44:43 +0100 Subject: [PATCH 75/99] [opticflow] more cleanup --- conf/modules/cv_opticflow.xml | 17 ++++++++++------- .../opticflow/hover_stabilization.c | 7 ++++--- .../opticflow/hover_stabilization.h | 3 ++- .../modules/computer_vision/opticflow_module.c | 3 --- 4 files changed, 16 insertions(+), 14 deletions(-) diff --git a/conf/modules/cv_opticflow.xml b/conf/modules/cv_opticflow.xml index 0b9cce7ee2..808b556134 100644 --- a/conf/modules/cv_opticflow.xml +++ b/conf/modules/cv_opticflow.xml @@ -12,14 +12,17 @@ - Controller can hold position +
+ + + + + + + +
- - - - - - - + diff --git a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c index 59bc249f16..f4e32384f5 100644 --- a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c +++ b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c @@ -22,7 +22,8 @@ * @file modules/computer_vision/opticflow/hover_stabilization.c * @brief optical-flow based hovering for Parrot AR.Drone 2.0 * - * Sensors from vertical camera and IMU of 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 @@ -195,6 +196,6 @@ void run_opticflow_hover(void) 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, &Velx, &Vely, &Velx_Int, &Vely_Int, &cmd_euler.phi, - &cmd_euler.theta); + DOWNLINK_SEND_VISION_STABILIZATION(DefaultChannel, DefaultDevice, &Velx, &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 index 92a6baeb4d..3f14b09f15 100644 --- a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.h +++ b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.h @@ -22,7 +22,8 @@ * @file modules/computer_vision/opticflow/hover_stabilization.h * @brief optical-flow based hovering for Parrot AR.Drone 2.0 * - * Sensors from vertical camera and IMU of 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_ diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c index 7186c006c5..99e84f6950 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -31,9 +31,6 @@ // Navigate Based On Vision, needed to call init/run_hover_stabilization_onvision #include "opticflow/hover_stabilization.h" -// Paparazzi -#include "state.h" // for attitude - // Threaded computer vision #include From 374be658d76ce41b5243b0d9853922d5a4f1931e Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 29 Jan 2015 11:01:18 +0100 Subject: [PATCH 76/99] [opticflow] fix distance check --- .../modules/computer_vision/opticflow/visual_estimator.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c index 7b0288a44f..75654434e6 100644 --- a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c +++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c @@ -103,7 +103,7 @@ static void agl_cb(uint8_t sender_id, const float *distance); static void agl_cb(uint8_t sender_id __attribute__((unused)), const float *distance) { - if (distance > 0) { + if (*distance > 0) { estimator_agl = *distance; } } From 17a839e361df0f6da6a074753f15dcc923138203 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 29 Jan 2015 13:08:30 +0100 Subject: [PATCH 77/99] [opticflow] more cleanup --- .../modules/computer_vision/opticflow/visual_estimator.c | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c index 75654434e6..80e8a0d8f6 100644 --- a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c +++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c @@ -81,7 +81,7 @@ float distance2, min_distance, min_distance2; // Flow Derotation #define FLOW_DEROTATION -float curr_pitch, curr_roll, curr_yaw, prev_pitch, prev_roll; +float curr_pitch, curr_roll, prev_pitch, prev_roll; float cam_h, diff_roll, diff_pitch, OFx_trans, OFy_trans; // Lateral Velocity Computation @@ -136,7 +136,6 @@ void opticflow_plugin_init(unsigned int w, unsigned int h) prev_roll = 0.0; curr_pitch = 0.0; curr_roll = 0.0; - curr_yaw = 0.0; OFx_trans = 0.0; OFy_trans = 0.0; Velx = 0.0; @@ -157,6 +156,7 @@ void opticflow_plugin_run(unsigned char *frame) // ************************************************************************************* // Additional information from other sensors + // !!WARNING!! Accessing of the state interface is NOT tread safe!!! // ************************************************************************************* // Compute body velocities from ENU @@ -269,7 +269,6 @@ void opticflow_plugin_run(unsigned char *frame) // Flow Derotation curr_pitch = stateGetNedToBodyEulers_f()->theta; curr_roll = stateGetNedToBodyEulers_f()->phi; - curr_yaw = stateGetNedToBodyEulers_f()->psi; diff_pitch = (curr_pitch - prev_pitch) * imgHeight / FOV_H; diff_roll = (curr_roll - prev_roll) * imgWidth / FOV_W; From f9f007956441e2a82cc040a878ac6e14e1690f7a Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 29 Jan 2015 21:16:55 +0100 Subject: [PATCH 78/99] [conf] no global waypoints in rotorcraft_basic since users are likely to use this flight plan at their home location, don't add any global waypoints (using lat/lon) that won't be moved by GeoInit --- conf/flight_plans/rotorcraft_basic.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/conf/flight_plans/rotorcraft_basic.xml b/conf/flight_plans/rotorcraft_basic.xml index 772967481f..04da89ae02 100644 --- a/conf/flight_plans/rotorcraft_basic.xml +++ b/conf/flight_plans/rotorcraft_basic.xml @@ -12,7 +12,7 @@ - + From 7d6ecf150def4f887faa943beff268515bf60b80 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 29 Jan 2015 23:37:43 +0100 Subject: [PATCH 79/99] [ocaml] make bindings on timestampped messages optional --- sw/ground_segment/cockpit/gcs.ml | 4 +++- sw/ground_segment/cockpit/live.ml | 32 +++++++++++++++--------------- sw/ground_segment/cockpit/live.mli | 4 ++-- sw/ground_segment/tmtc/messages.ml | 16 ++++++++------- sw/ground_segment/tmtc/server.ml | 19 +++++++++++------- sw/lib/ocaml/pprz.ml | 15 +++++++------- sw/lib/ocaml/pprz.mli | 2 +- 7 files changed, 51 insertions(+), 41 deletions(-) diff --git a/sw/ground_segment/cockpit/gcs.ml b/sw/ground_segment/cockpit/gcs.ml index f31b8d5d10..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"; ] @@ -771,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 108c5726c1..9f89de3fd6 100644 --- a/sw/ground_segment/cockpit/live.ml +++ b/sw/ground_segment/cockpit/live.ml @@ -790,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 -> @@ -1375,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 -> @@ -1384,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 @@ -1399,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 @@ -1414,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 @@ -1437,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/tmtc/messages.ml b/sw/ground_segment/tmtc/messages.ml index 34601f4d3f..8462f71476 100644 --- a/sw/ground_segment/tmtc/messages.ml +++ b/sw/ground_segment/tmtc/messages.ml @@ -170,7 +170,7 @@ let one_page = fun sender class_name (notebook:GPack.notebook) (topnote:GPack.no 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 (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,10 +181,10 @@ 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 (ident, xml_class, Some sender) end in List.iter - (fun m -> ignore (P.message_bind (Xml.attrib m "name") get_one)) + (fun m -> ignore (P.message_bind ~timestamp (Xml.attrib m "name") get_one)) messages | _ -> let class_notebook = GPack.notebook ~tab_border:0 ~tab_pos:`LEFT () in @@ -192,8 +192,8 @@ let rec one_class = fun (notebook:GPack.notebook) (help_label:GObj.widget) (wind 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 @@ -206,9 +206,11 @@ 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 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" ] (fun x -> prerr_endline ("WARNING: don't do anything with "^x)) "Usage: "; @@ -243,7 +245,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) 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/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 From 60481e4fbce8cf5dd844162fe146c5d744c61a05 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Fri, 30 Jan 2015 13:31:22 +0100 Subject: [PATCH 80/99] [message] improve ivy perf by only waiting for ALIVE by default --- sw/ground_segment/tmtc/messages.ml | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/sw/ground_segment/tmtc/messages.ml b/sw/ground_segment/tmtc/messages.ml index 8462f71476..4769e96a5a 100644 --- a/sw/ground_segment/tmtc/messages.ml +++ b/sw/ground_segment/tmtc/messages.ml @@ -170,7 +170,7 @@ let one_page = fun sender class_name (notebook:GPack.notebook) (topnote:GPack.no in bind id display -let rec one_class = fun (notebook:GPack.notebook) (help_label:GObj.widget) (window:GWindow.window) timestamp (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,11 +181,12 @@ 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 timestamp (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 ~timestamp (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 @@ -194,7 +195,7 @@ let rec one_class = fun (notebook:GPack.notebook) (help_label:GObj.widget) (wind let bind, sender_name = match sender with 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 notebook help_label window bind m)) messages @@ -207,10 +208,12 @@ 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"; - "-timestamp", Arg.Set timestamp, "Bind to timestampped messages" ] + "-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: "; @@ -245,7 +248,7 @@ let _ = !classes in (* Insert the message classes in the notebook *) - List.iter (one_class notebook help_label#coerce window !timestamp) xml_classes; + List.iter (one_class notebook help_label#coerce window !timestamp !force) xml_classes; (** Start the main loop *) window#show (); From 71f61cc3f4fe5714f27ea42535f1183e15734d98 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Fri, 30 Jan 2015 21:25:34 +0100 Subject: [PATCH 81/99] [fix] call pprzlog_init if needed (and avoid segfault) --- conf/firmwares/subsystems/shared/sdlog.makefile | 2 +- sw/airborne/subsystems/datalink/downlink.c | 4 ++++ sw/airborne/subsystems/datalink/pprzlog_transport.h | 3 +++ 3 files changed, 8 insertions(+), 1 deletion(-) 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/sw/airborne/subsystems/datalink/downlink.c b/sw/airborne/subsystems/datalink/downlink.c index b3a33cef16..c18275773b 100644 --- a/sw/airborne/subsystems/datalink/downlink.c +++ b/sw/airborne/subsystems/datalink/downlink.c @@ -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/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 From 705a7df752fd3a905ee449f1af9dce174ba1865b Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 7 Dec 2014 17:00:09 +0100 Subject: [PATCH 82/99] [stm32] use desig_get_unique_id from libopencm3 --- sw/airborne/arch/stm32/usb_ser_hw.c | 30 ++--------------------------- sw/ext/libopencm3 | 2 +- sw/ext/luftboot | 2 +- 3 files changed, 4 insertions(+), 30 deletions(-) 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/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 From a2f55f61bd4d632922c6c31fd4dcde062d1d833d Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 2 Feb 2015 11:43:27 +0100 Subject: [PATCH 83/99] [fix] fix extra_dl and pprrzlog - call init function for extra_dl - add missing byte in log header --- conf/modules/extra_dl.xml | 1 + sw/airborne/modules/datalink/extra_pprz_dl.c | 11 +++++---- sw/airborne/modules/datalink/extra_pprz_dl.h | 2 ++ sw/airborne/modules/meteo/meteo_france_DAQ.c | 2 +- sw/airborne/subsystems/datalink/downlink.c | 2 +- .../subsystems/datalink/pprz_transport.c | 24 +++++++++---------- .../subsystems/datalink/pprz_transport.h | 2 +- .../subsystems/datalink/pprzlog_transport.c | 1 + 8 files changed, 25 insertions(+), 20 deletions(-) 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/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/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/subsystems/datalink/downlink.c b/sw/airborne/subsystems/datalink/downlink.c index c18275773b..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(); 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)); } From 6605676f776c6e14e0129a23bae65c0f92a882ff Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 2 Feb 2015 14:13:17 +0100 Subject: [PATCH 84/99] [python] messages: PPRZ_HOME --- .../python/messages_app/messagesframe.py | 11 ++++++++--- sw/lib/python/messages_xml_map.py | 7 ++++++- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/sw/ground_segment/python/messages_app/messagesframe.py b/sw/ground_segment/python/messages_app/messagesframe.py index 5db50f55c8..dcebadb365 100644 --- a/sw/ground_segment/python/messages_app/messagesframe.py +++ b/sw/ground_segment/python/messages_app/messagesframe.py @@ -1,12 +1,17 @@ import wx import sys -import os import time import threading -PPRZ_HOME = os.getenv("PAPARAZZI_HOME") -sys.path.append(PPRZ_HOME + "/sw/lib/python") +from os import path, getenv + +# 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_SRC", PPRZ_SRC) from ivy_msg_interface import IvyMessagesInterface import messages_xml_map diff --git a/sw/lib/python/messages_xml_map.py b/sw/lib/python/messages_xml_map.py index d29cd5b5f6..c24d7c5cc5 100755 --- a/sw/lib/python/messages_xml_map.py +++ b/sw/lib/python/messages_xml_map.py @@ -4,7 +4,12 @@ from __future__ import absolute_import, print_function import os -default_messages_file = '%s/conf/messages.xml' % os.getenv("PAPARAZZI_HOME") +# 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 = {} From aeeaa3efb38f3b334f569c85e61a3a0b2a5d8a71 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 2 Feb 2015 15:49:11 +0100 Subject: [PATCH 85/99] [python] messages: fix typo --- sw/ground_segment/python/messages_app/messagesframe.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sw/ground_segment/python/messages_app/messagesframe.py b/sw/ground_segment/python/messages_app/messagesframe.py index dcebadb365..d36fe44a7c 100644 --- a/sw/ground_segment/python/messages_app/messagesframe.py +++ b/sw/ground_segment/python/messages_app/messagesframe.py @@ -11,7 +11,7 @@ from os import path, getenv 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_SRC", PPRZ_SRC) +PPRZ_HOME = getenv("PAPARAZZI_HOME", PPRZ_SRC) from ivy_msg_interface import IvyMessagesInterface import messages_xml_map From bb98e8a9b86f024fade571aed3b345feb21774f4 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 2 Feb 2015 12:55:59 +0100 Subject: [PATCH 86/99] [python] refactor messages --- .../python/messages_app/messagesframe.py | 28 ++++----- sw/lib/python/ivy_msg_interface.py | 27 +++++--- sw/lib/python/pprz_msg/__init__.py | 0 sw/lib/python/pprz_msg/message.py | 62 +++++++++++++++++++ .../python/{ => pprz_msg}/messages_xml_map.py | 26 +++++++- 5 files changed, 120 insertions(+), 23 deletions(-) create mode 100644 sw/lib/python/pprz_msg/__init__.py create mode 100644 sw/lib/python/pprz_msg/message.py rename sw/lib/python/{ => pprz_msg}/messages_xml_map.py (80%) diff --git a/sw/ground_segment/python/messages_app/messagesframe.py b/sw/ground_segment/python/messages_app/messagesframe.py index d36fe44a7c..d51b26c02e 100644 --- a/sw/ground_segment/python/messages_app/messagesframe.py +++ b/sw/ground_segment/python/messages_app/messagesframe.py @@ -14,7 +14,7 @@ sys.path.append(PPRZ_SRC + "/sw/lib/python") PPRZ_HOME = getenv("PAPARAZZI_HOME", PPRZ_SRC) from ivy_msg_interface import IvyMessagesInterface -import messages_xml_map +from pprz_msg.message import PprzMessage WIDTH = 450 LABEL_WIDTH = 166 @@ -23,18 +23,16 @@ HEIGHT = 800 BORDER = 1 -class Message: +class Message(PprzMessage): def __init__(self, class_name, name): - messages_xml_map.parse_messages() - self.field_value = [] - self.field_names = messages_xml_map.message_dictionary[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: +class Aircraft(object): def __init__(self, ac_id): self.ac_id = ac_id self.messages = {} @@ -42,7 +40,7 @@ class Aircraft: class MessagesFrame(wx.Frame): - def message_recv(self, msg_class, msg_name, ac_id, values): + def message_recv(self, msg_class, msg_name, ac_id, msg): """Handle incoming messages Callback function for IvyMessagesInterface @@ -53,8 +51,8 @@ class MessagesFrame(wx.Frame): :type msg_name: str :param ac_id: aircraft id :type ac_id: int - :param values: message values - :type values: list + :param msg: message + :type msg: PprzMessage """ # only show messages of the requested class if msg_class != self.msg_class: @@ -62,7 +60,7 @@ class MessagesFrame(wx.Frame): if ac_id in self.aircrafts and msg_name in self.aircrafts[ac_id].messages: if time.time() - self.aircrafts[ac_id].messages[msg_name].last_seen < 0.2: return - wx.CallAfter(self.gui_update, msg_class, msg_name, ac_id, values) + wx.CallAfter(self.gui_update, msg_class, msg_name, ac_id, msg) def find_page(self, book, name): if book.GetPageCount() < 1: @@ -119,7 +117,7 @@ class MessagesFrame(wx.Frame): messages_book = aircraft.messages_book 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) @@ -129,7 +127,7 @@ class MessagesFrame(wx.Frame): 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 @@ -147,7 +145,7 @@ class MessagesFrame(wx.Frame): field_panel.SetSizer(grid_sizer) field_panel.Layout() - def gui_update(self, msg_class, msg_name, ac_id, 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) @@ -159,8 +157,8 @@ class MessagesFrame(wx.Frame): 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[msg_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, 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') diff --git a/sw/lib/python/ivy_msg_interface.py b/sw/lib/python/ivy_msg_interface.py index 62130146ae..f44a1c606f 100644 --- a/sw/lib/python/ivy_msg_interface.py +++ b/sw/lib/python/ivy_msg_interface.py @@ -6,9 +6,17 @@ 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") -class IvyMessagesInterface(): - def __init__(self, callback, init=True, verbose=True, bind_regex="(.*)"): +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 @@ -27,7 +35,7 @@ class IvyMessagesInterface(): except: pass - def init_ivy(self, init, bind_regex): + 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) @@ -56,13 +64,16 @@ class IvyMessagesInterface(): # check which message class it is # pass non-telemetry messages with ac_id 0 - if data[0] in ["ground", "ground_dl", "dl"]: + 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:])) - elif data[0] == "sim": - return else: try: ac_id = int(data[0]) @@ -74,4 +85,6 @@ class IvyMessagesInterface(): msg_class = "telemetry" msg_name = data[1] values = list(filter(None, data[2:])) - self.callback(msg_class, msg_name, ac_id, values) + msg = PprzMessage(msg_class, msg_name) + msg.set_values(values) + self.callback(msg_class, msg_name, ac_id, msg) 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..202ed6955f --- /dev/null +++ b/sw/lib/python/pprz_msg/message.py @@ -0,0 +1,62 @@ +""" +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_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/messages_xml_map.py b/sw/lib/python/pprz_msg/messages_xml_map.py similarity index 80% rename from sw/lib/python/messages_xml_map.py rename to sw/lib/python/pprz_msg/messages_xml_map.py index c24d7c5cc5..1a8cdb1294 100755 --- a/sw/lib/python/messages_xml_map.py +++ b/sw/lib/python/pprz_msg/messages_xml_map.py @@ -7,7 +7,7 @@ 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 @@ -61,6 +61,30 @@ def parse_messages(messages_file=default_messages_file): 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() From 155561db45c278bbffeb013a13a6478100451ccf Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 2 Feb 2015 15:13:32 +0100 Subject: [PATCH 87/99] [python] ivytoredis: publish msg as json --- .../python/ivytoredis/ivy_to_redis.py | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/sw/ground_segment/python/ivytoredis/ivy_to_redis.py b/sw/ground_segment/python/ivytoredis/ivy_to_redis.py index 33afb81975..7b48b7f9bb 100755 --- a/sw/ground_segment/python/ivytoredis/ivy_to_redis.py +++ b/sw/ground_segment/python/ivytoredis/ivy_to_redis.py @@ -9,8 +9,12 @@ import argparse import sys import os -PPRZ_HOME = os.getenv("PAPARAZZI_HOME") -sys.path.append(PPRZ_HOME + "/sw/lib/python") +# 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 @@ -25,7 +29,7 @@ class Ivy2RedisServer(): self.keep_running = True print("Connected to redis server %s on port %i" % (redishost, redisport)) - def message_recv(self, msg_class, msg_name, ac_id, values): + def message_recv(self, msg_class, msg_name, 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: @@ -33,10 +37,10 @@ class Ivy2RedisServer(): else: key = "{0}.{1}".format(msg_class, msg_name) if self.verbose: - print("received message, key=%s, values=%s" % (key, ' '.join(values))) + print("received message, key=%s, msg=%s" % (key, msg.to_json(payload_only=True))) sys.stdout.flush() - self.r.publish(key, values) - self.r.set(key, values) + 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: From 8c9579c5a7619691d635821e8788c7ee63fb46aa Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 2 Feb 2015 15:37:23 +0100 Subject: [PATCH 88/99] [python] ivy_msg_interface returns only ac_id and msg --- .../python/ivytoredis/ivy_to_redis.py | 6 +++--- .../python/messages_app/messagesframe.py | 14 +++++--------- sw/lib/python/ivy_msg_interface.py | 2 +- sw/lib/python/pprz_msg/message.py | 6 ++++++ 4 files changed, 15 insertions(+), 13 deletions(-) diff --git a/sw/ground_segment/python/ivytoredis/ivy_to_redis.py b/sw/ground_segment/python/ivytoredis/ivy_to_redis.py index 7b48b7f9bb..285bc508c4 100755 --- a/sw/ground_segment/python/ivytoredis/ivy_to_redis.py +++ b/sw/ground_segment/python/ivytoredis/ivy_to_redis.py @@ -29,13 +29,13 @@ class Ivy2RedisServer(): self.keep_running = True print("Connected to redis server %s on port %i" % (redishost, redisport)) - def message_recv(self, msg_class, msg_name, ac_id, msg): + 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_class, msg_name, ac_id) + key = "{0}.{1}.{2}".format(msg.get_classname(), msg.get_msgname(), ac_id) else: - key = "{0}.{1}".format(msg_class, msg_name) + 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() diff --git a/sw/ground_segment/python/messages_app/messagesframe.py b/sw/ground_segment/python/messages_app/messagesframe.py index d51b26c02e..4a1862ea69 100644 --- a/sw/ground_segment/python/messages_app/messagesframe.py +++ b/sw/ground_segment/python/messages_app/messagesframe.py @@ -40,27 +40,23 @@ class Aircraft(object): class MessagesFrame(wx.Frame): - def message_recv(self, msg_class, msg_name, ac_id, msg): + def message_recv(self, ac_id, msg): """Handle incoming messages Callback function for IvyMessagesInterface - :param msg_class: message classe ("ground" or "telemetry") - :param msg_class: string - :param msg_name: message name - :type msg_name: str :param ac_id: aircraft id :type ac_id: int :param msg: message :type msg: PprzMessage """ # only show messages of the requested class - if msg_class != self.msg_class: + if msg.get_classname() != self.msg_class: return - if ac_id in self.aircrafts and msg_name in self.aircrafts[ac_id].messages: - if time.time() - self.aircrafts[ac_id].messages[msg_name].last_seen < 0.2: + 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_class, msg_name, ac_id, msg) + wx.CallAfter(self.gui_update, msg.get_classname(), msg.get_msgname(), ac_id, msg) def find_page(self, book, name): if book.GetPageCount() < 1: diff --git a/sw/lib/python/ivy_msg_interface.py b/sw/lib/python/ivy_msg_interface.py index f44a1c606f..245f16d91a 100644 --- a/sw/lib/python/ivy_msg_interface.py +++ b/sw/lib/python/ivy_msg_interface.py @@ -87,4 +87,4 @@ class IvyMessagesInterface(object): values = list(filter(None, data[2:])) msg = PprzMessage(msg_class, msg_name) msg.set_values(values) - self.callback(msg_class, msg_name, ac_id, msg) + self.callback(ac_id, msg) diff --git a/sw/lib/python/pprz_msg/message.py b/sw/lib/python/pprz_msg/message.py index 202ed6955f..2c81411488 100644 --- a/sw/lib/python/pprz_msg/message.py +++ b/sw/lib/python/pprz_msg/message.py @@ -27,6 +27,12 @@ class PprzMessage(object): 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 From f188864aa52c551b9b9a1543be60af9ed28805a6 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 2 Feb 2015 22:48:46 +0100 Subject: [PATCH 89/99] [opticflow] compute V_body in hover_stabilization --- .../opticflow/hover_stabilization.c | 8 ++++++++ .../computer_vision/opticflow/visual_estimator.c | 15 +++------------ .../computer_vision/opticflow/visual_estimator.h | 1 - 3 files changed, 11 insertions(+), 13 deletions(-) diff --git a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c index f4e32384f5..e3185b8f91 100644 --- a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c +++ b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c @@ -153,6 +153,14 @@ void run_hover_stabilization_onvision(void) void run_opticflow_hover(void) { + 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); + } + if (flow_count) { Error_Velx = Velx - vision_desired_vx; Error_Vely = Vely - vision_desired_vy; diff --git a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c index 80e8a0d8f6..3012b92ef6 100644 --- a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c +++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c @@ -87,8 +87,6 @@ float cam_h, diff_roll, diff_pitch, OFx_trans, OFy_trans; // Lateral Velocity Computation float Velx, Vely; -struct FloatVect3 V_body; - /** height above ground level, from ABI * Used for scale computation, negative value means invalid. */ @@ -154,16 +152,6 @@ void opticflow_plugin_run(unsigned char *frame) old_img_init = 0; } - // ************************************************************************************* - // Additional information from other sensors - // !!WARNING!! Accessing of the state interface is NOT tread safe!!! - // ************************************************************************************* - - // 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); - // ************************************************************************************* // Corner detection // ************************************************************************************* @@ -267,6 +255,7 @@ void opticflow_plugin_run(unsigned char *frame) } // Flow Derotation + // !!WARNING!! Accessing of the state interface is NOT tread safe!!! curr_pitch = stateGetNedToBodyEulers_f()->theta; curr_roll = stateGetNedToBodyEulers_f()->phi; @@ -320,11 +309,13 @@ void opticflow_plugin_run(unsigned char *frame) memcpy(prev_frame, frame, imgHeight * imgWidth * 2); memcpy(prev_gray_frame, gray_frame, imgHeight * imgWidth); +#if 0 // ************************************************************************************* // Downlink Message // ************************************************************************************* DOWNLINK_SEND_OF_HOVER(DefaultChannel, DefaultDevice, &FPS, &dx_sum, &dy_sum, &OFx, &OFy, &diff_roll, &diff_pitch, &Velx, &Vely, &V_body.x, &V_body.y, &cam_h, &count); +#endif } diff --git a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h index 5526a8676a..a5fce5296e 100644 --- a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h +++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h @@ -31,7 +31,6 @@ // Variables used by the controller extern float Velx, Vely; extern int flow_count; -extern struct FloatVect3 V_body; /** * Initialize visual estimator. From dfa33e4824db5e23386e6e5e361c398218d265fd Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 2 Feb 2015 22:53:29 +0100 Subject: [PATCH 90/99] [opticflow] example using unix domain sockets to return thread results --- .../computer_vision/opticflow_module.c | 51 ++++++++++++++++--- 1 file changed, 45 insertions(+), 6 deletions(-) diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c index 99e84f6950..247409ce7d 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -37,10 +37,25 @@ // Frame Rate (FPS) #include +// Sockets +#include +#include +#include +#include + #define USEC_PER_SEC 1000000L float FPS; + +struct CVresults { + int x; +}; + + +int cv_sockets[2]; + + // local variables volatile long timestamp; struct timeval start_time; @@ -91,6 +106,13 @@ void opticflow_module_run(void) { // Read Latest Vision Module Results if (computervision_thread_has_results) { + struct CVresults res; + if (read(cv_sockets[0], &res, sizeof(res))) { + perror("Failed to read CV results from socket.\n"); + } + /* TODO: use results */ + printf("result x=%i", res.x); + computervision_thread_has_results = 0; run_hover_stabilization_onvision(); } @@ -120,9 +142,11 @@ void opticflow_module_run(void) pthread_t computervision_thread; volatile uint8_t computervision_thread_status = 0; volatile uint8_t computer_vision_thread_command = 0; -void *computervision_thread_main(void *data); -void *computervision_thread_main(void *data) +void *computervision_thread_main(void *args); +void *computervision_thread_main(void *args) { + int socket = *(int *) args; + // Video Input struct vid_struct vid; vid.device = (char *)"/dev/video2"; // video1 = front camera; video2 = bottom camera @@ -165,6 +189,14 @@ void *computervision_thread_main(void *data) // Run Image Processing opticflow_plugin_run(img_new->buf); + /* send results to main */ + struct CVresults data; + /* TODO: fill in results here */ + data.x = 42; + if (write(socket, &data, sizeof(data))) { + printf("failed to write to socket.\n"); + } + #ifdef DOWNLINK_VIDEO // JPEG encode the image: uint32_t quality_factor = 10; //20 if no resize, @@ -187,10 +219,17 @@ void *computervision_thread_main(void *data) void opticflow_module_start(void) { - computer_vision_thread_command = 1; - int rc = pthread_create(&computervision_thread, NULL, computervision_thread_main, NULL); - if (rc) { - printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc); + /* create socket pair for communication */ + if (socketpair(AF_UNIX, SOCK_DGRAM, 0, cv_sockets) == 0) { + computer_vision_thread_command = 1; + 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"); } } From fc1d1093df346988e9ff475ee2f190e3f9ee39a2 Mon Sep 17 00:00:00 2001 From: dewagter Date: Wed, 4 Feb 2015 14:26:27 +0100 Subject: [PATCH 91/99] [module] video_usb_logger closes #1086 --- conf/modules/video_usb_logger.xml | 24 +++++ .../computer_vision/video_usb_logger.c | 88 +++++++++++++++++++ .../computer_vision/video_usb_logger.h | 34 +++++++ .../modules/computer_vision/viewvideo.c | 37 ++++++-- .../modules/computer_vision/viewvideo.h | 1 + sw/airborne/modules/loggers/file_logger.c | 8 +- 6 files changed, 180 insertions(+), 12 deletions(-) create mode 100644 conf/modules/video_usb_logger.xml create mode 100644 sw/airborne/modules/computer_vision/video_usb_logger.c create mode 100644 sw/airborne/modules/computer_vision/video_usb_logger.h 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/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/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 */ From ec745f02649337cb45c03f67d9b2c4ee5167e10c Mon Sep 17 00:00:00 2001 From: dewagter Date: Thu, 5 Feb 2015 17:28:50 +0100 Subject: [PATCH 92/99] convert to thread-communication --- conf/modules/cv_opticflow.xml | 4 +- .../modules/computer_vision/cv/framerate.c | 54 +++++ .../modules/computer_vision/cv/framerate.h | 5 + .../opticflow/hover_stabilization.c | 21 +- .../opticflow/hover_stabilization.h | 3 +- .../opticflow/inter_thread_data.h | 28 +++ .../opticflow/opticflow_thread.c | 121 ++++++++++ .../opticflow/opticflow_thread.h | 32 +++ .../opticflow/visual_estimator.c | 92 +++----- .../opticflow/visual_estimator.h | 9 +- .../computer_vision/opticflow_module.c | 213 +++++------------- 11 files changed, 345 insertions(+), 237 deletions(-) create mode 100644 sw/airborne/modules/computer_vision/cv/framerate.c create mode 100644 sw/airborne/modules/computer_vision/cv/framerate.h create mode 100644 sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h create mode 100644 sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c create mode 100644 sw/airborne/modules/computer_vision/opticflow/opticflow_thread.h diff --git a/conf/modules/cv_opticflow.xml b/conf/modules/cv_opticflow.xml index 808b556134..af3f680a58 100644 --- a/conf/modules/cv_opticflow.xml +++ b/conf/modules/cv_opticflow.xml @@ -43,16 +43,18 @@ - + + + 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..4fc3b4e282 --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv/framerate.c @@ -0,0 +1,54 @@ + +#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 + +float FPS; + +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 + FPS = 0.0; + timestamp = 0; + start_timer(); +} + +void framerate_run(void) { + // FPS + timestamp = end_timer(); + FPS = (float) 1000000 / (float)timestamp; + // printf("dt = %d, FPS = %f\n",timestamp, FPS); + start_timer(); +} 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..060811fff6 --- /dev/null +++ b/sw/airborne/modules/computer_vision/cv/framerate.h @@ -0,0 +1,5 @@ + +extern float FPS; + +void framerate_init(void); +void framerate_run(void); diff --git a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c index e3185b8f91..4dfee89e59 100644 --- a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c +++ b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c @@ -29,11 +29,7 @@ // Own Header #include "hover_stabilization.h" -// Vision Data -#include "visual_estimator.h" - // Stabilization -//#include "stabilization.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude.h" #include "firmwares/rotorcraft/guidance/guidance_v.h" #include "autopilot.h" @@ -144,15 +140,12 @@ void init_hover_stabilization_onvision() Vely_Int = 0; } -void run_hover_stabilization_onvision(void) +void run_hover_stabilization_onvision(struct CVresults* vision ) { - if (autopilot_mode == AP_MODE_MODULE) { - run_opticflow_hover(); + if (autopilot_mode != AP_MODE_MODULE) { + return; } -} -void run_opticflow_hover(void) -{ struct FloatVect3 V_body; if (activate_opticflow_hover == TRUE) { // Compute body velocities from ENU @@ -161,9 +154,9 @@ void run_opticflow_hover(void) float_quat_vmult(&V_body, q_n2b, vel_ned); } - if (flow_count) { - Error_Velx = Velx - vision_desired_vx; - Error_Vely = Vely - vision_desired_vy; + 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; @@ -204,6 +197,6 @@ void run_opticflow_hover(void) 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, &Velx, &Vely, &Velx_Int, + 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 index 3f14b09f15..285ac70864 100644 --- a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.h +++ b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.h @@ -30,6 +30,7 @@ #define HOVER_STABILIZATION_H_ #include +#include "inter_thread_data.h" // Controller module @@ -46,7 +47,7 @@ extern void guidance_h_module_run(bool_t in_flight); void init_hover_stabilization_onvision(void); -void run_hover_stabilization_onvision(void); +void run_hover_stabilization_onvision(struct CVresults *vision); extern bool activate_opticflow_hover; extern float vision_desired_vx; 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..512f75bb46 --- /dev/null +++ b/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h @@ -0,0 +1,28 @@ + + +#ifndef _INTER_THREAD_DATA_H +#define _INTER_THREAD_DATA_H + + +// Inter-thread communication: Unix Socket + +// Data from thread to module +struct CVresults { + int x; + int status; + float Velx; + float Vely; + int flow_count; +}; + +// Data from module to thread +struct PPRZinfo { + int cnt; + float theta; + float phi; + float agl; +}; + + + +#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..00d9a0f275 --- /dev/null +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c @@ -0,0 +1,121 @@ + + + +// 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 + +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 = 0; +} + +void *computervision_thread_main(void *args) +{ + int thread_socket = *(int *) args; + int cnt = 0; + + computer_vision_thread_command = RUN; + + struct CVresults vision_results; + struct PPRZinfo autopilot_data; + + // Video Input + struct vid_struct vid; + vid.device = (char *)"/dev/video2"; // video1 = front camera; video2 = bottom camera + vid.w = 320; + vid.h = 240; + vid.n_buffers = 4; + + vision_results.status = 1; + + if (video_init(&vid) < 0) { + printf("Error initialising video\n"); + vision_results.status = -1; + 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 > 0) { + vision_results.status = 2; + video_grab_image(&vid, img_new); + + // Get most recent State + int bytes_read = read(thread_socket, &autopilot_data, sizeof(autopilot_data)); + if (bytes_read <= sizeof(autopilot_data)) { + if (bytes_read != 0) { + printf("Failed to read PPRZ info from socket.\n"); + } + } + + // Run Image Processing + opticflow_plugin_run(img_new->buf, &autopilot_data, &vision_results); + + /* send results to main */ + vision_results.x = cnt++; + int bytes_written = write(thread_socket, &vision_results, sizeof(vision_results)); + if (bytes_written != sizeof(vision_results)){ + perror("failed to write to socket.\n"); + } + + +#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); + uint32_t delta_t_per_frame = 0; // 0 = use drone clock + send_rtp_frame(vsock, jpegbuf, size, small.w, small.h, 0, quality_factor, dri_header, delta_t_per_frame); +#endif + } + printf("Thread Closed\n"); + video_close(&vid); + vision_results.status = -100; + 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..eaa48a1d38 --- /dev/null +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.h @@ -0,0 +1,32 @@ +/* + * 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 + * @brief computer vision thread + * + * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 + */ + + +#include "std.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); diff --git a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c index 3012b92ef6..f69d8aa6b8 100644 --- a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c +++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c @@ -25,7 +25,10 @@ * 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 +#include #include // Own Header @@ -36,17 +39,7 @@ #include "opticflow/fast9/fastRosten.h" // for FPS -#include "modules/computer_vision/opticflow_module.h" - -// Paparazzi Data -#include "state.h" -#include "subsystems/abi.h" - -// Downlink -#include "subsystems/datalink/downlink.h" - -// Timer -#include +#include "modules/computer_vision/cv/framerate.h" // Image size set at init unsigned int imgWidth, imgHeight; @@ -71,7 +64,6 @@ int max_count = 25; // Corner Tracking int *new_x, *new_y, *status, *dx, *dy; int error_opticflow; -int flow_count = 0; int remove_point; int c; int borderx = 24, bordery = 24; @@ -84,30 +76,10 @@ float distance2, min_distance, min_distance2; float curr_pitch, curr_roll, prev_pitch, prev_roll; float cam_h, diff_roll, diff_pitch, OFx_trans, OFy_trans; -// Lateral Velocity Computation -float Velx, Vely; -/** height above ground level, from ABI - * Used for scale computation, negative value means invalid. - */ -volatile float estimator_agl; - -/** 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, const float *distance); - -static void agl_cb(uint8_t sender_id __attribute__((unused)), const float *distance) -{ - if (*distance > 0) { - estimator_agl = *distance; - } -} // Called by plugin -void opticflow_plugin_init(unsigned int w, unsigned int h) +void opticflow_plugin_init(unsigned int w, unsigned int h, struct CVresults *results) { // Initialize variables imgWidth = w; @@ -136,16 +108,17 @@ void opticflow_plugin_init(unsigned int w, unsigned int h) curr_roll = 0.0; OFx_trans = 0.0; OFy_trans = 0.0; - Velx = 0.0; - Vely = 0.0; + results->Velx = 0.0; + results->Vely = 0.0; + results->flow_count = 0; - // get AGL from sonar via ABI - estimator_agl = -1.0; - AbiBindMsgAGL(OPTICFLOW_AGL_ID, &agl_ev, agl_cb); + framerate_init(); } -void opticflow_plugin_run(unsigned char *frame) +void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CVresults *results) { + framerate_run(); + if (old_img_init == 1) { memcpy(prev_frame, frame, imgHeight * imgWidth * 2); CvtYUYV2Gray(prev_gray_frame, prev_frame, imgWidth, imgHeight); @@ -213,7 +186,7 @@ void opticflow_plugin_run(unsigned char *frame) error_opticflow = opticFlowLK(gray_frame, prev_gray_frame, x, y, count_fil, imgWidth, imgHeight, new_x, new_y, status, 5, 100); - flow_count = count_fil; + results->flow_count = count_fil; for (int i = count_fil - 1; i >= 0; i--) { remove_point = 1; @@ -223,13 +196,13 @@ void opticflow_plugin_run(unsigned char *frame) } if (remove_point) { - for (c = i; c < flow_count - 1; c++) { + for (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]; } - flow_count--; + results->flow_count--; } } @@ -237,27 +210,26 @@ void opticflow_plugin_run(unsigned char *frame) dy_sum = 0.0; // Optical Flow Computation - for (int i = 0; i < flow_count; i++) { + 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 (flow_count) { - quick_sort_int(dx, flow_count); // 11 - quick_sort_int(dy, flow_count); // 11 + if (results->flow_count) { + quick_sort_int(dx, results->flow_count); // 11 + quick_sort_int(dy, results->flow_count); // 11 - dx_sum = (float) dx[flow_count / 2]; - dy_sum = (float) dy[flow_count / 2]; + dx_sum = (float) dx[results->flow_count / 2]; + dy_sum = (float) dy[results->flow_count / 2]; } else { dx_sum = 0.0; dy_sum = 0.0; } // Flow Derotation - // !!WARNING!! Accessing of the state interface is NOT tread safe!!! - curr_pitch = stateGetNedToBodyEulers_f()->theta; - curr_roll = stateGetNedToBodyEulers_f()->phi; + curr_pitch = info->theta; + curr_roll = info->phi; diff_pitch = (curr_pitch - prev_pitch) * imgHeight / FOV_H; diff_roll = (curr_roll - prev_roll) * imgWidth / FOV_W; @@ -266,7 +238,7 @@ void opticflow_plugin_run(unsigned char *frame) prev_roll = curr_roll; #ifdef FLOW_DEROTATION - if (flow_count) { + if (results->flow_count) { OFx_trans = dx_sum - diff_roll; OFy_trans = dy_sum - diff_pitch; @@ -284,22 +256,22 @@ void opticflow_plugin_run(unsigned char *frame) #endif // Average Filter - OFfilter(&OFx, &OFy, OFx_trans, OFy_trans, flow_count, 1); + OFfilter(&OFx, &OFy, OFx_trans, OFy_trans, results->flow_count, 1); // Velocity Computation - if (estimator_agl < 0) { + if (info->agl < 0) { cam_h = 1; } else { - cam_h = estimator_agl; + cam_h = info->agl; } - if (flow_count) { - Velx = OFy * cam_h * FPS / Fy_ARdrone + 0.05; - Vely = -OFx * cam_h * FPS / Fx_ARdrone - 0.1; + if (results->flow_count) { + results->Velx = OFy * cam_h * FPS / Fy_ARdrone + 0.05; + results->Vely = -OFx * cam_h * FPS / Fx_ARdrone - 0.1; } else { - Velx = 0.0; - Vely = 0.0; + results->Velx = 0.0; + results->Vely = 0.0; } // ************************************************************************************* diff --git a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h index a5fce5296e..5fef180b6d 100644 --- a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h +++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h @@ -28,16 +28,15 @@ #ifndef VISUAL_ESTIMATOR_H #define VISUAL_ESTIMATOR_H -// Variables used by the controller -extern float Velx, Vely; -extern int flow_count; + +#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); -void opticflow_plugin_run(unsigned char *frame); +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 index 247409ce7d..e47810b308 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -28,200 +28,101 @@ #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 -// Frame Rate (FPS) -#include - // Sockets #include #include #include #include -#define USEC_PER_SEC 1000000L - -float FPS; - - -struct CVresults { - int x; -}; - - int cv_sockets[2]; +// Paparazzi Data +#include "state.h" +#include "subsystems/abi.h" -// local variables -volatile long timestamp; -struct timeval start_time; -struct timeval end_time; +// Downlink +#include "subsystems/datalink/downlink.h" -static long time_elapsed(struct timeval *t1, struct timeval *t2) + +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, const float *distance); + +static void agl_cb(uint8_t sender_id __attribute__((unused)), const float *distance) { - 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; + if (*distance > 0) { + opticflow_module_data.agl = *distance; } - 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 opticflow_module_init(void) { - // Immediately start the vision thread when the module initialized - opticflow_module_start(); + // 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(); - - // Frame Rate Initialization - FPS = 0.0; - timestamp = 0; - start_timer(); } -volatile uint8_t computervision_thread_has_results = 0; - 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)){ + perror("module failed to write to socket.\n"); + } + // Read Latest Vision Module Results - if (computervision_thread_has_results) { - struct CVresults res; - if (read(cv_sockets[0], &res, sizeof(res))) { - perror("Failed to read CV results from socket.\n"); + struct CVresults vision_results; + int bytes_read = read(cv_sockets[0], &vision_results, sizeof(vision_results)); + if (bytes_read <= sizeof(vision_results)) { + if (bytes_read != 0) { + printf("Failed to read CV results from socket.\n"); } - /* TODO: use results */ - printf("result x=%i", res.x); - - computervision_thread_has_results = 0; - run_hover_stabilization_onvision(); + } else { + //////////////////////////////////////////// + // Module-Side Code + //////////////////////////////////////////// + run_hover_stabilization_onvision(&vision_results); } } - -///////////////////////////////////////////////////////////////////////// -// COMPUTER VISION THREAD - -// Video -#include "v4l/video.h" -#include "resize.h" - -// Payload Code -#include "opticflow/visual_estimator.h" - -// Downlink Video -//#define DOWNLINK_VIDEO 1 - -#ifdef DOWNLINK_VIDEO -#include "encoding/jpeg.h" -#include "encoding/rtp.h" -#endif - -#include - -pthread_t computervision_thread; -volatile uint8_t computervision_thread_status = 0; -volatile uint8_t computer_vision_thread_command = 0; -void *computervision_thread_main(void *args); -void *computervision_thread_main(void *args) -{ - int socket = *(int *) args; - - // Video Input - struct vid_struct vid; - vid.device = (char *)"/dev/video2"; // video1 = front camera; video2 = bottom camera - vid.w = 320; - vid.h = 240; - vid.n_buffers = 4; - - if (video_init(&vid) < 0) { - printf("Error initialising video\n"); - computervision_thread_status = -1; - 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); - - while (computer_vision_thread_command > 0) { - video_grab_image(&vid, img_new); - - // FPS - timestamp = end_timer(); - FPS = (float) 1000000 / (float)timestamp; - // printf("dt = %d, FPS = %f\n",timestamp, FPS); - start_timer(); - - // Run Image Processing - opticflow_plugin_run(img_new->buf); - - /* send results to main */ - struct CVresults data; - /* TODO: fill in results here */ - data.x = 42; - if (write(socket, &data, sizeof(data))) { - printf("failed to write to socket.\n"); - } - -#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); - uint32_t delta_t_per_frame = 0; // 0 = use drone clock - send_rtp_frame(vsock, jpegbuf, size, small.w, small.h, 0, quality_factor, dri_header, delta_t_per_frame); -#endif - computervision_thread_has_results++; - } - printf("Thread Closed\n"); - video_close(&vid); - computervision_thread_status = -100; - return 0; -} - void opticflow_module_start(void) { - /* create socket pair for communication */ + pthread_t computervision_thread; if (socketpair(AF_UNIX, SOCK_DGRAM, 0, cv_sockets) == 0) { - computer_vision_thread_command = 1; + //////////////////////////////////////////// + // Thread-Side Code + //////////////////////////////////////////// int rc = pthread_create(&computervision_thread, NULL, computervision_thread_main, &cv_sockets[1]); if (rc) { @@ -235,5 +136,5 @@ void opticflow_module_start(void) void opticflow_module_stop(void) { - computer_vision_thread_command = 0; + computervision_thread_request_exit(); } From 143154882cd7be040db9388b7087745edf970681 Mon Sep 17 00:00:00 2001 From: dewagter Date: Thu, 5 Feb 2015 17:30:02 +0100 Subject: [PATCH 93/99] fix thread communication --- .../opticflow/inter_thread_data.h | 2 +- .../opticflow/opticflow_thread.c | 22 ++++++++++++------- .../opticflow/visual_estimator.c | 2 ++ .../computer_vision/opticflow_module.c | 16 +++++++++----- 4 files changed, 28 insertions(+), 14 deletions(-) diff --git a/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h b/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h index 512f75bb46..71f74b9f16 100644 --- a/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h +++ b/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h @@ -8,7 +8,7 @@ // Data from thread to module struct CVresults { - int x; + int cnt; int status; float Velx; float Vely; diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c index 00d9a0f275..03b77acf45 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c @@ -39,7 +39,6 @@ void computervision_thread_request_exit(void) { void *computervision_thread_main(void *args) { int thread_socket = *(int *) args; - int cnt = 0; computer_vision_thread_command = RUN; @@ -78,27 +77,34 @@ void *computervision_thread_main(void *args) // First Apply Settings before init opticflow_plugin_init(vid.w, vid.h, &vision_results); - while (computer_vision_thread_command > 0) { + while (computer_vision_thread_command == RUN) { vision_results.status = 2; video_grab_image(&vid, img_new); // Get most recent State - int bytes_read = read(thread_socket, &autopilot_data, sizeof(autopilot_data)); - if (bytes_read <= sizeof(autopilot_data)) { - if (bytes_read != 0) { - printf("Failed to read PPRZ info from socket.\n"); + 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); + } } } + printf("[thread] Read # %d\n",autopilot_data.cnt); + // Run Image Processing opticflow_plugin_run(img_new->buf, &autopilot_data, &vision_results); /* send results to main */ - vision_results.x = cnt++; + vision_results.cnt++; int bytes_written = write(thread_socket, &vision_results, sizeof(vision_results)); if (bytes_written != sizeof(vision_results)){ - perror("failed to write to socket.\n"); + perror("[thread] Failed to write to socket.\n"); } + printf("[thread] Write # %d, (bytes %d)\n",vision_results.cnt, bytes_written); #ifdef DOWNLINK_VIDEO diff --git a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c index f69d8aa6b8..59908d0e3f 100644 --- a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c +++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c @@ -111,6 +111,8 @@ void opticflow_plugin_init(unsigned int w, unsigned int h, struct CVresults *res results->Velx = 0.0; results->Vely = 0.0; results->flow_count = 0; + results->cnt = 0; + results->status = 0; framerate_init(); } diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c index e47810b308..81998103b5 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -39,6 +39,7 @@ #include // Sockets +#include #include #include #include @@ -98,20 +99,25 @@ void opticflow_module_run(void) 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)){ - perror("module failed to write to socket.\n"); + printf("[module] Failed to write to socket: written = %d, error=%d.\n",bytes_written, errno); + } + else { + printf("[module] Write # %d (%d bytes)\n",opticflow_module_data.cnt, bytes_written); } // Read Latest Vision Module Results struct CVresults vision_results; - int bytes_read = read(cv_sockets[0], &vision_results, sizeof(vision_results)); - if (bytes_read <= sizeof(vision_results)) { - if (bytes_read != 0) { - printf("Failed to read CV results from socket.\n"); + // 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 //////////////////////////////////////////// + printf("[module] Read vision %d\n",vision_results.cnt); run_hover_stabilization_onvision(&vision_results); } } From 558927473310bee7172001f77c4f8edba4e4e90c Mon Sep 17 00:00:00 2001 From: dewagter Date: Thu, 5 Feb 2015 18:21:34 +0100 Subject: [PATCH 94/99] Messages in module instead of thread --- .../modules/computer_vision/cv/framerate.c | 34 +++++++- .../modules/computer_vision/cv/framerate.h | 26 +++++- .../opticflow/hover_stabilization.c | 19 ++++- .../opticflow/inter_thread_data.h | 5 ++ .../opticflow/opticflow_thread.c | 9 ++- .../opticflow/opticflow_thread.h | 2 - .../opticflow/visual_estimator.c | 81 +++++++++---------- .../computer_vision/opticflow_module.c | 5 +- .../computer_vision/opticflow_module.h | 3 - 9 files changed, 122 insertions(+), 62 deletions(-) diff --git a/sw/airborne/modules/computer_vision/cv/framerate.c b/sw/airborne/modules/computer_vision/cv/framerate.c index 4fc3b4e282..c3fec4e2a7 100644 --- a/sw/airborne/modules/computer_vision/cv/framerate.c +++ b/sw/airborne/modules/computer_vision/cv/framerate.c @@ -1,3 +1,27 @@ +/* + * 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 + * + */ #include "std.h" #include "framerate.h" @@ -12,7 +36,11 @@ struct timeval end_time; #define USEC_PER_SEC 1000000L -float FPS; +static float framerate_FPS; + +float framerate_get(void) { + return framerate_FPS; +} static long time_elapsed(struct timeval *t1, struct timeval *t2) { @@ -40,7 +68,7 @@ static long end_timer(void) void framerate_init(void) { // Frame Rate Initialization - FPS = 0.0; + framerate_FPS = 0.0; timestamp = 0; start_timer(); } @@ -48,7 +76,7 @@ void framerate_init(void) { void framerate_run(void) { // FPS timestamp = end_timer(); - FPS = (float) 1000000 / (float)timestamp; + framerate_FPS = (float) 1000000 / (float)timestamp; // printf("dt = %d, FPS = %f\n",timestamp, FPS); start_timer(); } diff --git a/sw/airborne/modules/computer_vision/cv/framerate.h b/sw/airborne/modules/computer_vision/cv/framerate.h index 060811fff6..47b236f179 100644 --- a/sw/airborne/modules/computer_vision/cv/framerate.h +++ b/sw/airborne/modules/computer_vision/cv/framerate.h @@ -1,5 +1,29 @@ +/* + * 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 + * + */ -extern float FPS; void framerate_init(void); void framerate_run(void); +float framerate_get(void); diff --git a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c index 4dfee89e59..8394ba32ec 100644 --- a/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c +++ b/sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c @@ -142,10 +142,6 @@ void init_hover_stabilization_onvision() void run_hover_stabilization_onvision(struct CVresults* vision ) { - if (autopilot_mode != AP_MODE_MODULE) { - return; - } - struct FloatVect3 V_body; if (activate_opticflow_hover == TRUE) { // Compute body velocities from ENU @@ -154,6 +150,21 @@ void run_hover_stabilization_onvision(struct CVresults* vision ) 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; diff --git a/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h b/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h index 71f74b9f16..eed79db6e0 100644 --- a/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h +++ b/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h @@ -10,9 +10,14 @@ struct CVresults { int cnt; int status; + float FPS; float Velx; float Vely; int flow_count; + float cam_h; + int count; + float OFx, OFy, dx_sum, dy_sum; + float diff_roll, diff_pitch; }; // Data from module to thread diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c index 03b77acf45..cf0ad3db34 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c @@ -30,10 +30,13 @@ #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 = 0; + computer_vision_thread_command = EXIT; } void *computervision_thread_main(void *args) @@ -93,7 +96,7 @@ void *computervision_thread_main(void *args) } } - printf("[thread] Read # %d\n",autopilot_data.cnt); + DEBUG_INFO("[thread] Read # %d\n",autopilot_data.cnt); // Run Image Processing opticflow_plugin_run(img_new->buf, &autopilot_data, &vision_results); @@ -104,7 +107,7 @@ void *computervision_thread_main(void *args) if (bytes_written != sizeof(vision_results)){ perror("[thread] Failed to write to socket.\n"); } - printf("[thread] Write # %d, (bytes %d)\n",vision_results.cnt, bytes_written); + DEBUG_INFO("[thread] Write # %d, (bytes %d)\n",vision_results.cnt, bytes_written); #ifdef DOWNLINK_VIDEO diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.h b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.h index eaa48a1d38..157de7273c 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.h +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.h @@ -26,7 +26,5 @@ */ -#include "std.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); diff --git a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c index 59908d0e3f..b560427f6e 100644 --- a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c +++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c @@ -47,7 +47,6 @@ unsigned int imgWidth, imgHeight; // Local variables unsigned char *prev_frame, *gray_frame, *prev_gray_frame; int old_img_init; -float OFx, OFy, dx_sum, dy_sum; // ARDrone Vertical Camera Parameters #define FOV_H 0.67020643276 @@ -57,7 +56,6 @@ float OFx, OFy, dx_sum, dy_sum; // Corner Detection int *x, *y; -int count = 0; int max_count = 25; #define MAX_COUNT 100 @@ -74,7 +72,7 @@ float distance2, min_distance, min_distance2; // Flow Derotation #define FLOW_DEROTATION float curr_pitch, curr_roll, prev_pitch, prev_roll; -float cam_h, diff_roll, diff_pitch, OFx_trans, OFy_trans; +float OFx_trans, OFy_trans; @@ -95,13 +93,13 @@ void opticflow_plugin_init(unsigned int w, unsigned int h, struct CVresults *res dx = (int *) calloc(MAX_COUNT, sizeof(int)); dy = (int *) calloc(MAX_COUNT, sizeof(int)); old_img_init = 1; - OFx = 0.0; - OFy = 0.0; - dx_sum = 0.0; - dy_sum = 0.0; - diff_roll = 0.0; - diff_pitch = 0.0; - cam_h = 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; prev_pitch = 0.0; prev_roll = 0.0; curr_pitch = 0.0; @@ -113,6 +111,7 @@ void opticflow_plugin_init(unsigned int w, unsigned int h, struct CVresults *res results->flow_count = 0; results->cnt = 0; results->status = 0; + results->count = 0; framerate_init(); } @@ -135,10 +134,10 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV int fast_threshold = 20; xyFAST *pnts_fast; pnts_fast = fast9_detect((const byte *)prev_gray_frame, imgWidth, imgHeight, imgWidth, - fast_threshold, &count); + fast_threshold, &results->count); - if (count > MAX_COUNT) { count = MAX_COUNT; } - for (int i = 0; i < count; i++) { + 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; } @@ -149,8 +148,8 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV min_distance2 = min_distance * min_distance; int *labelmin; labelmin = (int *) calloc(MAX_COUNT, sizeof(int)); - for (int i = 0; i < count; i++) { - for (int j = i + 1; j < count; j++) { + for (int i = 0; i < results->count; i++) { + for (int j = i + 1; j < results->count; j++) { // distance squared: distance2 = (x[i] - x[j]) * (x[i] - x[j]) + (y[i] - y[j]) * (y[i] - y[j]); if (distance2 < min_distance2) { @@ -159,8 +158,8 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV } } - int count_fil = count; - for (int i = count - 1; i >= 0; i--) { + int count_fil = results->count; + for (int i = results->count - 1; i >= 0; i--) { remove_point = 0; if (labelmin[i]) { @@ -177,7 +176,7 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV } if (count_fil > max_count) { count_fil = max_count; } - count = count_fil; + results->count = count_fil; free(labelmin); // ************************************************************************************* @@ -208,8 +207,8 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV } } - dx_sum = 0.0; - dy_sum = 0.0; + results->dx_sum = 0.0; + results->dy_sum = 0.0; // Optical Flow Computation for (int i = 0; i < results->flow_count; i++) { @@ -222,35 +221,35 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV quick_sort_int(dx, results->flow_count); // 11 quick_sort_int(dy, results->flow_count); // 11 - dx_sum = (float) dx[results->flow_count / 2]; - dy_sum = (float) dy[results->flow_count / 2]; + results->dx_sum = (float) dx[results->flow_count / 2]; + results->dy_sum = (float) dy[results->flow_count / 2]; } else { - dx_sum = 0.0; - dy_sum = 0.0; + results->dx_sum = 0.0; + results->dy_sum = 0.0; } // Flow Derotation curr_pitch = info->theta; curr_roll = info->phi; - diff_pitch = (curr_pitch - prev_pitch) * imgHeight / FOV_H; - diff_roll = (curr_roll - prev_roll) * imgWidth / FOV_W; + results->diff_pitch = (curr_pitch - prev_pitch) * imgHeight / FOV_H; + results->diff_roll = (curr_roll - prev_roll) * imgWidth / FOV_W; prev_pitch = curr_pitch; prev_roll = curr_roll; #ifdef FLOW_DEROTATION if (results->flow_count) { - OFx_trans = dx_sum - diff_roll; - OFy_trans = dy_sum - diff_pitch; + OFx_trans = results->dx_sum - results->diff_roll; + OFy_trans = results->dy_sum - results->diff_pitch; - if ((OFx_trans <= 0) != (dx_sum <= 0)) { + if ((OFx_trans <= 0) != (results->dx_sum <= 0)) { OFx_trans = 0; OFy_trans = 0; } } else { - OFx_trans = dx_sum; - OFy_trans = dy_sum; + OFx_trans = results->dx_sum; + OFy_trans = results->dy_sum; } #else OFx_trans = dx_sum; @@ -258,19 +257,21 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV #endif // Average Filter - OFfilter(&OFx, &OFy, OFx_trans, OFy_trans, results->flow_count, 1); + OFfilter(&results->OFx, &results->OFy, OFx_trans, OFy_trans, results->flow_count, 1); // Velocity Computation if (info->agl < 0) { - cam_h = 1; + results->cam_h = 1; } else { - cam_h = info->agl; + results->cam_h = info->agl; } + results->FPS = framerate_get(); + if (results->flow_count) { - results->Velx = OFy * cam_h * FPS / Fy_ARdrone + 0.05; - results->Vely = -OFx * cam_h * FPS / Fx_ARdrone - 0.1; + 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; @@ -283,13 +284,5 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV memcpy(prev_frame, frame, imgHeight * imgWidth * 2); memcpy(prev_gray_frame, gray_frame, imgHeight * imgWidth); -#if 0 - // ************************************************************************************* - // Downlink Message - // ************************************************************************************* - DOWNLINK_SEND_OF_HOVER(DefaultChannel, DefaultDevice, &FPS, &dx_sum, &dy_sum, &OFx, &OFy, - &diff_roll, &diff_pitch, &Velx, &Vely, &V_body.x, &V_body.y, - &cam_h, &count); -#endif } diff --git a/sw/airborne/modules/computer_vision/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c index 81998103b5..c8805c9eca 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -74,6 +74,7 @@ static void agl_cb(uint8_t sender_id __attribute__((unused)), const float *dista } } +#define DEBUG_INFO(X, ...) ; void opticflow_module_init(void) { @@ -102,7 +103,7 @@ void opticflow_module_run(void) printf("[module] Failed to write to socket: written = %d, error=%d.\n",bytes_written, errno); } else { - printf("[module] Write # %d (%d bytes)\n",opticflow_module_data.cnt, bytes_written); + DEBUG_INFO("[module] Write # %d (%d bytes)\n",opticflow_module_data.cnt, bytes_written); } // Read Latest Vision Module Results @@ -117,7 +118,7 @@ void opticflow_module_run(void) //////////////////////////////////////////// // Module-Side Code //////////////////////////////////////////// - printf("[module] Read vision %d\n",vision_results.cnt); + DEBUG_INFO("[module] Read vision %d\n",vision_results.cnt); run_hover_stabilization_onvision(&vision_results); } } diff --git a/sw/airborne/modules/computer_vision/opticflow_module.h b/sw/airborne/modules/computer_vision/opticflow_module.h index 4f6e394e04..098d0b15b3 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.h +++ b/sw/airborne/modules/computer_vision/opticflow_module.h @@ -36,7 +36,4 @@ extern void opticflow_module_run(void); extern void opticflow_module_start(void); extern void opticflow_module_stop(void); -/// Frame Rate -extern float FPS; - #endif /* OPTICFLOW_MODULE_H */ From 9dd169c09c6447b90ec532779db6255808d80041 Mon Sep 17 00:00:00 2001 From: TU Delft developer Date: Thu, 5 Feb 2015 20:39:06 +0100 Subject: [PATCH 95/99] Added new RC USB type joystick from Hobbyking --- .../hobbyking_usb_rc_6ch_rc_tx_joy_mode_2.xml | 50 +++++++++++++++++++ 1 file changed, 50 insertions(+) create mode 100644 conf/joystick/hobbyking_usb_rc_6ch_rc_tx_joy_mode_2.xml 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..1d18ddc878 --- /dev/null +++ b/conf/joystick/hobbyking_usb_rc_6ch_rc_tx_joy_mode_2.xml @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + From 11d94d76c2b8e109fad23cc3a07baa395308d647 Mon Sep 17 00:00:00 2001 From: dewagter Date: Thu, 5 Feb 2015 21:13:43 +0100 Subject: [PATCH 96/99] [opticflow] working variables in a struct --- .../modules/computer_vision/cv/framerate.c | 14 +- .../modules/computer_vision/cv/framerate.h | 3 +- .../opticflow/inter_thread_data.h | 22 +-- .../opticflow/opticflow_thread.c | 51 ++++-- .../opticflow/opticflow_thread.h | 5 +- .../opticflow/visual_estimator.c | 152 +++++++++--------- 6 files changed, 128 insertions(+), 119 deletions(-) diff --git a/sw/airborne/modules/computer_vision/cv/framerate.c b/sw/airborne/modules/computer_vision/cv/framerate.c index c3fec4e2a7..9ec262d0bc 100644 --- a/sw/airborne/modules/computer_vision/cv/framerate.c +++ b/sw/airborne/modules/computer_vision/cv/framerate.c @@ -19,7 +19,7 @@ */ /** - * @file modules/computer_vision/cv/framerate.h + * @file modules/computer_vision/cv/framerate.c * */ @@ -36,12 +36,6 @@ struct timeval end_time; #define USEC_PER_SEC 1000000L -static float framerate_FPS; - -float framerate_get(void) { - return framerate_FPS; -} - static long time_elapsed(struct timeval *t1, struct timeval *t2) { long sec, usec; @@ -68,15 +62,15 @@ static long end_timer(void) void framerate_init(void) { // Frame Rate Initialization - framerate_FPS = 0.0; timestamp = 0; start_timer(); } -void framerate_run(void) { +float framerate_run(void) { // FPS timestamp = end_timer(); - framerate_FPS = (float) 1000000 / (float)timestamp; + 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 index 47b236f179..9044881ff0 100644 --- a/sw/airborne/modules/computer_vision/cv/framerate.h +++ b/sw/airborne/modules/computer_vision/cv/framerate.h @@ -25,5 +25,4 @@ void framerate_init(void); -void framerate_run(void); -float framerate_get(void); +float framerate_run(void); diff --git a/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h b/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h index eed79db6e0..e28f44ab2f 100644 --- a/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h +++ b/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h @@ -8,24 +8,26 @@ // Data from thread to module struct CVresults { - int cnt; - int status; - float FPS; - float Velx; + int cnt; // Number of processed frames + + float Velx; // Velocity as measured by camera float Vely; int flow_count; - float cam_h; + + float cam_h; // Debug parameters int count; float OFx, OFy, dx_sum, dy_sum; - float diff_roll, diff_pitch; + float diff_roll; + float diff_pitch; + float FPS; }; // Data from module to thread struct PPRZinfo { - int cnt; - float theta; - float phi; - float agl; + int cnt; // IMU msg counter + float phi; // roll [rad] + float theta; // pitch [rad] + float agl; // height above ground [m] }; diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c index cf0ad3db34..d5d8e46e7f 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c @@ -1,3 +1,27 @@ +/* + * 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 + * + */ @@ -29,10 +53,8 @@ #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) { @@ -43,11 +65,13 @@ void *computervision_thread_main(void *args) { int thread_socket = *(int *) args; - computer_vision_thread_command = RUN; - + // Local data in/out struct CVresults vision_results; struct PPRZinfo autopilot_data; + // Status + computer_vision_thread_command = RUN; + // Video Input struct vid_struct vid; vid.device = (char *)"/dev/video2"; // video1 = front camera; video2 = bottom camera @@ -55,11 +79,8 @@ void *computervision_thread_main(void *args) vid.h = 240; vid.n_buffers = 4; - vision_results.status = 1; - if (video_init(&vid) < 0) { printf("Error initialising video\n"); - vision_results.status = -1; return 0; } @@ -81,10 +102,11 @@ void *computervision_thread_main(void *args) opticflow_plugin_init(vid.w, vid.h, &vision_results); while (computer_vision_thread_command == RUN) { - vision_results.status = 2; + + // Wait for a new frame video_grab_image(&vid, img_new); - // Get most recent State + // Get most recent State information int bytes_read = sizeof(autopilot_data); while (bytes_read == sizeof(autopilot_data)) { @@ -95,13 +117,12 @@ void *computervision_thread_main(void *args) } } } - DEBUG_INFO("[thread] Read # %d\n",autopilot_data.cnt); - // Run Image Processing + // Run Image Processing with image and data and get results opticflow_plugin_run(img_new->buf, &autopilot_data, &vision_results); - /* send results to main */ + /* Send results to main */ vision_results.cnt++; int bytes_written = write(thread_socket, &vision_results, sizeof(vision_results)); if (bytes_written != sizeof(vision_results)){ @@ -109,7 +130,6 @@ void *computervision_thread_main(void *args) } 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, @@ -119,12 +139,11 @@ void *computervision_thread_main(void *args) uint32_t size = end - (jpegbuf); printf("Sending an image ...%u\n", size); - uint32_t delta_t_per_frame = 0; // 0 = use drone clock - send_rtp_frame(vsock, jpegbuf, size, small.w, small.h, 0, quality_factor, dri_header, delta_t_per_frame); + send_rtp_frame(vsock, jpegbuf, size, small.w, small.h, 0, quality_factor, dri_header, 0); #endif } + printf("Thread Closed\n"); video_close(&vid); - vision_results.status = -100; return 0; } diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.h b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.h index 157de7273c..06ad218cae 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.h +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.h @@ -19,11 +19,10 @@ */ /** - * @file modules/computer_vision/opticflow/opticflow_thread.c + * @file modules/computer_vision/opticflow/opticflow_thread.h * @brief computer vision thread * - * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 - */ + */ void *computervision_thread_main(void *args); /* computer vision thread: should be given a pointer to a socketpair as argument */ diff --git a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c index b560427f6e..cb10364ca7 100644 --- a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c +++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c @@ -27,6 +27,8 @@ // Warning: all this code is called form the Vision-Thread: do not access any autopilot data in here. +#include "std.h" + #include #include #include @@ -41,12 +43,26 @@ // for FPS #include "modules/computer_vision/cv/framerate.h" -// Image size set at init -unsigned int imgWidth, imgHeight; // Local variables -unsigned char *prev_frame, *gray_frame, *prev_gray_frame; -int old_img_init; +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 @@ -55,44 +71,27 @@ int old_img_init; #define Fy_ARdrone 348.5053 // Corner Detection -int *x, *y; -int max_count = 25; #define MAX_COUNT 100 -// Corner Tracking -int *new_x, *new_y, *status, *dx, *dy; -int error_opticflow; -int remove_point; -int c; -int borderx = 24, bordery = 24; - -// Remove bad corners -float distance2, min_distance, min_distance2; - // Flow Derotation #define FLOW_DEROTATION -float curr_pitch, curr_roll, prev_pitch, prev_roll; -float OFx_trans, OFy_trans; - // Called by plugin void opticflow_plugin_init(unsigned int w, unsigned int h, struct CVresults *results) { // Initialize variables - imgWidth = w; - imgHeight = h; - gray_frame = (unsigned char *) calloc(imgWidth * imgHeight, sizeof(unsigned char)); - prev_frame = (unsigned char *) calloc(imgWidth * imgHeight * 2, sizeof(unsigned char)); - prev_gray_frame = (unsigned char *) calloc(imgWidth * imgHeight, sizeof(unsigned char)); - x = (int *) calloc(MAX_COUNT, sizeof(int)); - new_x = (int *) calloc(MAX_COUNT, sizeof(int)); - y = (int *) calloc(MAX_COUNT, sizeof(int)); - new_y = (int *) calloc(MAX_COUNT, sizeof(int)); - status = (int *) calloc(MAX_COUNT, sizeof(int)); - dx = (int *) calloc(MAX_COUNT, sizeof(int)); - dy = (int *) calloc(MAX_COUNT, sizeof(int)); - old_img_init = 1; + 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; @@ -100,17 +99,10 @@ void opticflow_plugin_init(unsigned int w, unsigned int h, struct CVresults *res results->diff_roll = 0.0; results->diff_pitch = 0.0; results->cam_h = 0.0; - prev_pitch = 0.0; - prev_roll = 0.0; - curr_pitch = 0.0; - curr_roll = 0.0; - OFx_trans = 0.0; - OFy_trans = 0.0; results->Velx = 0.0; results->Vely = 0.0; results->flow_count = 0; results->cnt = 0; - results->status = 0; results->count = 0; framerate_init(); @@ -118,12 +110,24 @@ void opticflow_plugin_init(unsigned int w, unsigned int h, struct CVresults *res void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CVresults *results) { - framerate_run(); + // 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; - if (old_img_init == 1) { - memcpy(prev_frame, frame, imgHeight * imgWidth * 2); - CvtYUYV2Gray(prev_gray_frame, prev_frame, imgWidth, imgHeight); - old_img_init = 0; + // 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; } // ************************************************************************************* @@ -133,9 +137,8 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV // FAST corner detection int fast_threshold = 20; xyFAST *pnts_fast; - pnts_fast = fast9_detect((const byte *)prev_gray_frame, imgWidth, imgHeight, imgWidth, + 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; @@ -143,15 +146,14 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV } free(pnts_fast); - // Remove neighbouring corners - min_distance = 3; - min_distance2 = min_distance * min_distance; - int *labelmin; - labelmin = (int *) calloc(MAX_COUNT, sizeof(int)); + // 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: - distance2 = (x[i] - x[j]) * (x[i] - x[j]) + (y[i] - y[j]) * (y[i] - y[j]); + 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; } @@ -160,14 +162,14 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV int count_fil = results->count; for (int i = results->count - 1; i >= 0; i--) { - remove_point = 0; + int remove_point = 0; if (labelmin[i]) { remove_point = 1; } if (remove_point) { - for (c = i; c < count_fil - 1; c++) { + for (int c = i; c < count_fil - 1; c++) { x[c] = x[c + 1]; y[c] = y[c + 1]; } @@ -177,27 +179,26 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV if (count_fil > max_count) { count_fil = max_count; } results->count = count_fil; - free(labelmin); // ************************************************************************************* // Corner Tracking // ************************************************************************************* - CvtYUYV2Gray(gray_frame, frame, imgWidth, imgHeight); + CvtYUYV2Gray(visual_estimator.gray_frame, frame, w, h); - error_opticflow = opticFlowLK(gray_frame, prev_gray_frame, x, y, count_fil, imgWidth, - imgHeight, new_x, new_y, status, 5, 100); + 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--) { - remove_point = 1; + int remove_point = 1; - if (status[i] && !(new_x[i] < borderx || new_x[i] > (imgWidth - 1 - borderx) || - new_y[i] < bordery || new_y[i] > (imgHeight - 1 - bordery))) { + 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 (c = i; c < results->flow_count - 1; c++) { + 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]; @@ -229,15 +230,12 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV } // Flow Derotation - curr_pitch = info->theta; - curr_roll = info->phi; - - results->diff_pitch = (curr_pitch - prev_pitch) * imgHeight / FOV_H; - results->diff_roll = (curr_roll - prev_roll) * imgWidth / FOV_W; - - prev_pitch = curr_pitch; - prev_roll = curr_roll; + 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; @@ -252,23 +250,21 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV OFy_trans = results->dy_sum; } #else - OFx_trans = dx_sum; - OFy_trans = dy_sum; + 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) { - results->cam_h = 1; + if (info->agl < 0.01) { + results->cam_h = 0.01; } else { results->cam_h = info->agl; } - results->FPS = framerate_get(); - 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; @@ -281,8 +277,8 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV // Next Loop Preparation // ************************************************************************************* - memcpy(prev_frame, frame, imgHeight * imgWidth * 2); - memcpy(prev_gray_frame, gray_frame, imgHeight * imgWidth); + memcpy(visual_estimator.prev_frame, frame, w * h * 2); + memcpy(visual_estimator.prev_gray_frame, visual_estimator.gray_frame, w * h); } From 5fd3808a39b2025eca85d175bea637e84b3340d6 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 5 Feb 2015 21:59:46 +0100 Subject: [PATCH 97/99] [opticflow] dox --- .../opticflow/inter_thread_data.h | 35 +++++++++++++++---- .../opticflow/opticflow_thread.c | 7 ++-- .../opticflow/opticflow_thread.h | 6 +++- .../opticflow/visual_estimator.c | 13 ++++--- .../opticflow/visual_estimator.h | 5 ++- 5 files changed, 45 insertions(+), 21 deletions(-) diff --git a/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h b/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h index e28f44ab2f..65ef0cb41e 100644 --- a/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h +++ b/sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h @@ -1,12 +1,35 @@ +/* + * 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 - -// Inter-thread communication: Unix Socket - -// Data from thread to module +/// Data from thread to module struct CVresults { int cnt; // Number of processed frames @@ -22,7 +45,7 @@ struct CVresults { float FPS; }; -// Data from module to thread +/// Data from module to thread struct PPRZinfo { int cnt; // IMU msg counter float phi; // roll [rad] @@ -30,6 +53,4 @@ struct PPRZinfo { 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 index d5d8e46e7f..0566279f1b 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c @@ -23,8 +23,6 @@ * */ - - // Sockets #include #include @@ -74,7 +72,10 @@ void *computervision_thread_main(void *args) // Video Input struct vid_struct vid; - vid.device = (char *)"/dev/video2"; // video1 = front camera; video2 = bottom camera + /* On ARDrone2: + * video1 = front camera; video2 = bottom camera + */ + vid.device = (char *)"/dev/video2"; vid.w = 320; vid.h = 240; vid.n_buffers = 4; diff --git a/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.h b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.h index 06ad218cae..5826da2229 100644 --- a/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.h +++ b/sw/airborne/modules/computer_vision/opticflow/opticflow_thread.h @@ -22,8 +22,12 @@ * @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 index cb10364ca7..48bd4ffd5d 100644 --- a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c +++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.c @@ -20,13 +20,13 @@ /** * @file modules/computer_vision/opticflow/visual_estimator.c - * @brief optical-flow based hovering for Parrot AR.Drone 2.0 + * @brief Estimate velocity from optic flow. * - * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 + * 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. */ -// Warning: all this code is called form the Vision-Thread: do not access any autopilot data in here. - #include "std.h" #include @@ -185,8 +185,8 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV // ************************************************************************************* 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); + 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--) { @@ -281,4 +281,3 @@ void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CV 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 index 5fef180b6d..f499ffd320 100644 --- a/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h +++ b/sw/airborne/modules/computer_vision/opticflow/visual_estimator.h @@ -20,15 +20,14 @@ /** * @file modules/computer_vision/opticflow/visual_estimator.h - * @brief optical-flow based hovering for Parrot AR.Drone 2.0 + * @brief Estimate velocity from optic flow. * - * Sensors from vertical camera and IMU of Parrot AR.Drone 2.0 + * 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" /** From 953b1a605d50b409dd197260da3a4be4d5b1923d Mon Sep 17 00:00:00 2001 From: TU Delft developer Date: Fri, 6 Feb 2015 10:06:34 +0100 Subject: [PATCH 98/99] Sign of pitch roll axis changed and yaw fit --- conf/joystick/hobbyking_usb_rc_6ch_rc_tx_joy_mode_2.xml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 index 1d18ddc878..b778a15957 100644 --- 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 @@ -42,9 +42,9 @@ The basis of steering is the standard signs of aerospace convention - - + + - + \ No newline at end of file From 30df14eb05c37d1cccddf3d8f685dd3de698efce Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 5 Feb 2015 17:57:53 +0100 Subject: [PATCH 99/99] [abi] allow to pass variables by value before the generated ABI callbacks always had a signature with `const type *var` where type was e.g float. Now the generated signature is simply `type var`. To pass const pointers again, set it accordingly in abi.xml, e.g. type="const float *" instead of type="float" --- sw/airborne/boards/apogee/baro_board.c | 4 ++-- sw/airborne/boards/ardrone/baro_board.c | 4 ++-- sw/airborne/boards/ardrone/navdata.c | 2 +- sw/airborne/boards/baro_board_ms5611_i2c.c | 4 ++-- sw/airborne/boards/baro_board_ms5611_spi.c | 4 ++-- sw/airborne/boards/booz/baro_board.c | 2 +- sw/airborne/boards/hbmini/baro_board.c | 2 +- sw/airborne/boards/lisa_l/baro_board.c | 4 ++-- sw/airborne/boards/lisa_m/baro_board.c | 4 ++-- sw/airborne/boards/navgo/baro_board.c | 2 +- sw/airborne/boards/navstik/baro_board.c | 4 ++-- sw/airborne/boards/umarim/baro_board.c | 2 +- sw/airborne/modules/air_data/air_data.c | 12 +++++------ .../computer_vision/opticflow_module.c | 8 ++++---- sw/airborne/modules/meteo/meteo_stick.c | 4 ++-- .../modules/sensors/airspeed_ms45xx_i2c.c | 4 ++-- sw/airborne/modules/sensors/baro_MS5534A.c | 2 +- sw/airborne/modules/sensors/baro_amsys.c | 2 +- sw/airborne/modules/sensors/baro_bmp.c | 4 ++-- sw/airborne/modules/sensors/baro_ets.c | 2 +- sw/airborne/modules/sensors/baro_hca.c | 2 +- sw/airborne/modules/sensors/baro_mpl3115.c | 4 ++-- sw/airborne/modules/sensors/baro_ms5611_i2c.c | 4 ++-- sw/airborne/modules/sensors/baro_ms5611_spi.c | 4 ++-- sw/airborne/modules/sensors/baro_scp.c | 2 +- sw/airborne/modules/sensors/baro_scp_i2c.c | 2 +- sw/airborne/modules/sensors/baro_sim.c | 2 +- .../modules/sensors/pressure_board_navarro.c | 2 +- sw/airborne/modules/sonar/agl_dist.c | 8 ++++---- sw/airborne/modules/sonar/sonar_adc.c | 2 +- sw/airborne/subsystems/ins/ins_alt_float.c | 10 +++++----- .../subsystems/ins/ins_float_invariant.c | 14 ++++++------- sw/airborne/subsystems/ins/ins_int.c | 20 +++++++++---------- sw/airborne/test/test_baro_board.c | 4 ++-- sw/simulator/nps/nps_autopilot_fixedwing.c | 2 +- sw/simulator/nps/nps_autopilot_rotorcraft.c | 4 ++-- sw/tools/generators/gen_abi.ml | 4 ++-- 37 files changed, 83 insertions(+), 83 deletions(-) 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 be5e5be248..e791cdb8e6 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/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/opticflow_module.c b/sw/airborne/modules/computer_vision/opticflow_module.c index c8805c9eca..3687f65e2c 100644 --- a/sw/airborne/modules/computer_vision/opticflow_module.c +++ b/sw/airborne/modules/computer_vision/opticflow_module.c @@ -65,12 +65,12 @@ struct PPRZinfo opticflow_module_data; #define OPTICFLOW_AGL_ID ABI_BROADCAST #endif abi_event agl_ev; -static void agl_cb(uint8_t sender_id, const float *distance); +static void agl_cb(uint8_t sender_id, float distance); -static void agl_cb(uint8_t sender_id __attribute__((unused)), const float *distance) +static void agl_cb(uint8_t sender_id __attribute__((unused)), float distance) { - if (*distance > 0) { - opticflow_module_data.agl = *distance; + if (distance > 0) { + opticflow_module_data.agl = distance; } } 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/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/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 521e8182a7..4955f35404 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant.c +++ b/sw/airborne/subsystems/ins/ins_float_invariant.c @@ -180,7 +180,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); /* gps */ bool_t ins_gps_fix_once; @@ -501,7 +501,7 @@ void ahrs_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; @@ -513,10 +513,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 @@ -525,12 +525,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/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/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