paparazzi-devel
[Top][All Lists]
Advanced

[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]

Re: [Paparazzi-devel] Problem compiling with a custom written subroutine


From: Felix Ruess
Subject: Re: [Paparazzi-devel] Problem compiling with a custom written subroutine (zamboni survey)
Date: Wed, 13 Mar 2013 22:42:42 +0100

Don't see it right now... from just looking at what you posted.
I can offer to have a look at if if you commit/push it to github...

Felix

On Sun, Mar 10, 2013 at 9:47 PM, Jorn Anke <address@hidden> wrote:
Hi Felix,

Yes, the error-message is still the same. The zamboni_survey.h file is located in the subsystems/navigation folder and looks like quoted under.

Cheers,

Jorn

-------------------- zamboni_survey.h ---------------------
#ifndef ZAMBONI_H
#define ZAMBONI_H

#include "std.h"

//typedef struct {float x; float y;} point2d;
typedef enum {Z_ERR, Z_ENTRY, Z_SEG, Z_TURN1, Z_RET, Z_TURN2} z_survey_stage;


extern bool_t init_zamboni_survey(uint8_t center_wp, uint8_t dir_wp, float sweep_length, float sweep_spacing, int sweep_lines, float altitude);
extern bool_t zamboni_survey(void);

#endif


-------------------- flight plan ---------------------

<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">

<flight_plan alt="310" ground_alt="185" lat0="59.81" lon0="10.3578" max_dist_from_home="550" name="Test zamboni" security_height="60" qfu="110">
  <header>
#include "subsystems/navigation/zamboni_survey.h"
#include "subsystems/datalink/datalink.h"
/*
#ifndef VARDECLARED
#define VARDECLARED
float varsweepsize=110;
#endif
*/
#include "modules/digital_cam/dc.h"
//#define LINE_START_FUNCTION dc_autoshoot = DC_AUTOSHOOT_DISTANCE;
//#define LINE_START_FUNCTION dc_Survey(dc_gps_dist);
#define LINE_START_FUNCTION dc_Survey(40);
#define LINE_STOP_FUNCTION dc_autoshoot = DC_AUTOSHOOT_STOP;
</header>
  <waypoints>
    <waypoint name="HOME" x="-20" y="-40"/>
    <waypoint name="STDBY" x="-40" y="-60"/>
    <waypoint name="Z_CENTER" x="170" y="-90"/>
    <waypoint name="Z1_DIR" x="270" y="-120"/>
    <waypoint name="Z2_DIR" x="270" y="-120"/>
    <waypoint name="CLIMB" x="0" y="-70"/>
    <waypoint alt="240" name="AF" x="-180" y="160"/>
    <waypoint alt="170.0" name="TD" x="-20" y="10"/>
    <waypoint name="_BASELEG" x="1" y="1"/>
  </waypoints>
  <blocks>
    <block name="Wait GPS">
      <set value="1" var="kill_throttle"/>
      <while cond="!GpsFixValid()"/>
    </block>
    <block name="Geo init">
      <while cond="LessThan(NavBlockTime(), 10)"/>
      <call fun="NavSetGroundReferenceHere()"/>
    </block>
    <block name="Holding point">
      <set value="1" var="kill_throttle"/>
      <attitude roll="0" throttle="0" vmode="throttle"/>
    </block>
   
    <!-- New 2013.03.06, copied from basix.xml -->
    <block key="t" name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png" group="home">
      <exception cond="GetPosAlt() > ground_alt+25" deroute="Standby"/>
      <set value="0" var="kill_throttle"/>
      <set value="0" var="autopilot_flight_time"/>
      <go from="HOME" throttle="1.0" vmode="throttle" wp="CLIMB" pitch="15"/>
    </block>



   
    <!-- init_zamboni_survey(uint8_t center_wp, uint8_t dir_wp, float sweep_length, float sweep_spacing, int sweep_lines, float altitude) /-->
    <block name="ZamboniSurvey1" strip_button="Zamboni1" strip_icon="survey.png">
      <call fun="init_zamboni_survey(WP_Z_CENTER, WP_Z1_DIR, 200, 40, 7, 290)"/>
      <call fun="zamboni_survey()"/>
    </block>

    <!-- init_zamboni_survey(uint8_t center_wp, uint8_t dir_wp, float sweep_length, float sweep_spacing, int sweep_lines, float altitude) /-->
    <block name="ZamboniSurvey2" strip_button="Zamboni2" strip_icon="survey.png">
      <call fun="init_zamboni_survey(WP_Z_CENTER, WP_Z2_DIR, 200, -40, 7, 290)"/>
      <call fun="zamboni_survey()"/>
    </block>
   
<!-- New 2013.03.06, copied from basix.xml -->
    <block name="Land Right AF-TD" strip_button="Land right (wp AF-TD)" strip_icon="land-right.png" group="land">
      <set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
      <deroute block="land"/>
    </block>
    <block name="Land Left AF-TD" strip_button="Land left (wp AF-TD)" strip_icon="land-left.png" group="land">
      <set value="-DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
      <deroute block="land"/>
    </block>
    <block name="land">
      <call fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
      <circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
      <circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
    </block>
    <block name="final">
      <exception cond="ground_alt + 10 > GetPosAlt()" deroute="flare"/>
      <go from="AF" hmode="route" vmode="glide" wp="TD"/>
    </block>


    <block name="flare">
      <go approaching_time="0" from="AF" hmode="route" throttle="0.0" vmode="throttle" wp="TD"/>
      <attitude roll="0.0" throttle="0.0" until="FALSE" vmode="throttle"/>
    </block>
    <block group="home" key="<Control>a" name="Standby" strip_button="Standby" strip_icon="home.png">
      <circle radius="nav_radius" wp="STDBY"/>
    </block>
  </blocks>
</flight_plan>



-------------------- zamboni_survey.c ---------------------
#include "zamboni_survey.h"

#include "subsystems/nav.h"
#include "estimator.h"
#include "autopilot.h"
#include "generated/flight_plan.h"

#ifdef DIGITAL_CAM
#include "modules/digital_cam/dc.h"
#endif

int counter;
/**
variables used to store values from the flight plan
**/
float x_wp_center, y_wp_center;
float x_wp_dir, y_wp_dir;
float z_sweep_length;
float z_sweep_spacing;
int z_sweep_lines;
float z_shot_dist;
float z_altitude;

/**
static variables, used for initial calculations
**/
// properties for the filightpattern
float flight_angle, return_angle;
float dx_sweep_width, dy_sweep_width;
float dx_flightline, dy_flightline;
float dx_flight_vec, dy_flight_vec;
float turnradius1, turnradius2;
int laps;

// points for navigation
float x_seg_start, y_seg_start;
float x_seg_end, y_seg_end;
float x_turn_center1, y_turn_center1;
float x_turn_center2, y_turn_center2;
float x_ret_start, y_ret_start;
float x_ret_end, y_ret_end;

// variables used for initial calculations 
  float dx_flight_wpts, dy_flight_wpts;
  float len;
 
// constant for storing value for pre-leave-angle, (leave turncircles a small angle before the 180deg turns are compleated to get a smoother transition to flight-lines)
  int pre_leave_angle=2;

z_survey_stage z_stage;
/*
z_stage starts at ENTRY and than circles trought the other
states until to rectangle is completely covered
ENTRY : getting in the right position and height for the first flyover
SEG   : fly from seg_start to seg_end and take pictures,
        then calculate navigation points of next flyover
TURN1 : do a 180° turn around seg_center1
RET   : fly from ret_start to ret_end
TURN2 : do a 180° turn around seg_center2
*/

/**
 initializes the variables needed for the survey to start
   wp_center     :  the waypoint defining the center of the survey-rectangle
   wp_dir        :  the waypoint defining the orientation of the survey-rectangle
   sweep_length  :  the length of the survey-rectangle
   sweep_spacing :  distance between the sweeps
   sweep_lines   :  number of sweep_lines to fly
   altitude      :  the altitude that must be reached before the flyover starts
**/
bool_t init_zamboni_survey(uint8_t center_wp, uint8_t dir_wp, float sweep_length, float sweep_spacing, int sweep_lines, float altitude)
{
  counter = 0;
  // copy variables from the flight plan
  x_wp_center = waypoints[center_wp].x;
  y_wp_center = waypoints[center_wp].y;
  x_wp_dir = waypoints[dir_wp].x;
  y_wp_dir = waypoints[dir_wp].y;
  z_sweep_length = sweep_length;
  z_sweep_spacing = sweep_spacing;
  z_sweep_lines = sweep_lines;
  //z_shot_dist = shot_dist;
  z_altitude = altitude;
 
  // if turning right leave circle before angle is reached, if turning left - leave after
  if (z_sweep_spacing>0) pre_leave_angle -= pre_leave_angle;

  // calculate the flight_angle
  dx_flight_wpts = x_wp_dir - x_wp_center;
  dy_flight_wpts = y_wp_dir - y_wp_center;
  if (dy_flight_wpts == 0) dy_flight_wpts = 0.01; // to avoid dividing by zero
  flight_angle = 180*atan2(dx_flight_wpts, dy_flight_wpts)/M_PI;
  return_angle = flight_angle + 180;
  if (return_angle > 359) return_angle -= 360;
 
  // calculate the flightline vector from start to end of one flightline, (delta x and delta y for one flightline)
  // (used later to move the flight pattern one flightline up for each round)
  len = sqrtf(dx_flight_wpts * dx_flight_wpts + dy_flight_wpts * dy_flight_wpts);
  dx_flight_vec = dx_flight_wpts/len;
  dy_flight_vec = dy_flight_wpts/len;
  dx_flightline = dx_flight_vec * z_sweep_length;
  dy_flightline = dy_flight_vec * z_sweep_length;
 
  // calculate the vector from one flightline perpendicular to the next flightline left, seen in the flightdirection. (Delta x and delta y betwen two adjecent flightlines)
  // (used later to move the flight pattern one flightline up for each round)
  dx_sweep_width = -(dy_flight_wpts/len) * z_sweep_spacing;
  dy_sweep_width = +(dx_flight_wpts/len) * z_sweep_spacing;
   
  // calculate number of laps to fly and turning radius for each end
  laps = (z_sweep_lines+1)/2;
  turnradius1 = (laps-1) * z_sweep_spacing * 0.5;
  turnradius2 = laps * z_sweep_spacing * 0.5;
 
  //CALCULATE THE NAVIGATION POINTS
  //start and end of flight-line in flight-direction
  x_seg_start = x_wp_center - dx_flightline * 0.5;
  y_seg_start = y_wp_center - dy_flightline * 0.5;
  x_seg_end = x_wp_center + dx_flightline * 0.5;
  y_seg_end = y_wp_center + dy_flightline * 0.5;
 
  //start and end of flight-line in return-direction
  x_ret_start = x_seg_end - dx_sweep_width * (laps-1);
  y_ret_start = y_seg_end - dy_sweep_width * (laps-1);
  x_ret_end = x_seg_start - dx_sweep_width * (laps-1);
  y_ret_end = y_seg_start - dy_sweep_width * (laps-1);

  //turn-centers at both ends
  x_turn_center1 = (x_seg_end + x_ret_start)/2;
  y_turn_center1 = (y_seg_end + y_ret_start)/2;
  x_turn_center2 = (x_seg_start + x_ret_end + dx_sweep_width) / 2;
  y_turn_center2 = (y_seg_start + y_ret_end + dy_sweep_width) / 2;

  //fast climbing to desired altitude
  NavVerticalAutoThrottleMode(100.0);
  NavVerticalAltitudeMode(z_altitude, 0.0);
 
  z_stage = Z_ENTRY;
 
  return FALSE;
}

/**
   main navigation routine. This is called periodically evaluates the current
   Position and stage and navigates accordingly.
   Returns True until the survey is finished
**/
bool_t zamboni_survey(void)
{
  // retain altitude
  NavVerticalAutoThrottleMode(0.0);
  NavVerticalAltitudeMode(z_altitude, 0.0);
 
  //go from center of field to end of field - (before starting the syrvey)
  if (z_stage == Z_ENTRY) {
    nav_route_xy(x_wp_center, y_wp_center, x_seg_end, y_seg_end);
    if (nav_approaching_xy(x_seg_end, y_seg_end, x_wp_center, y_wp_center, CARROT)) {
      z_stage = Z_TURN1;
      NavVerticalAutoThrottleMode(0.0);
      nav_init_stage();
    }
  }
 
  //Turn from stage to return
  else if (z_stage == Z_TURN1) {
    nav_circle_XY(x_turn_center1, y_turn_center1, turnradius1);
    if (NavCourseCloseTo(return_angle+pre_leave_angle)){
        // && nav_approaching_xy(x_seg_end, y_seg_end, x_seg_start, y_seg_start, CARROT
      //calculate SEG-points for the next flyover
      x_seg_start = x_seg_start + dx_sweep_width;
      y_seg_start = y_seg_start + dy_sweep_width;
      x_seg_end = x_seg_end + dx_sweep_width;
      y_seg_end = y_seg_end + dy_sweep_width;
     
      z_stage = Z_RET;
      nav_init_stage();
      #ifdef DIGITAL_CAM
        //dc_survey(z_shot_dist, x_ret_start - dx_flight_vec * z_shot_dist, y_ret_start - dy_flight_vec * z_shot_dist);
        LINE_START_FUNCTION;
      #endif
    }
  }
 
  //fly the segment until seg_end is reached
  else if (z_stage == Z_RET) {
    nav_route_xy(x_ret_start, y_ret_start, x_ret_end, y_ret_end);
    if (nav_approaching_xy(x_ret_end, y_ret_end, x_ret_start, y_ret_start, 0)) {
      counter = counter + 1;
      #ifdef DIGITAL_CAM
        //dc_stop();
        LINE_STOP_FUNCTION;
      #endif
      z_stage = Z_TURN2;  
    }
  }

  //turn from stage to return
  else if (z_stage == Z_TURN2) {
    nav_circle_XY(x_turn_center2, y_turn_center2, turnradius2);
    if (NavCourseCloseTo(flight_angle+pre_leave_angle)) {
      //counter = counter + 1;
      z_stage = Z_SEG;
      nav_init_stage();
      #ifdef DIGITAL_CAM
        //dc_survey(z_shot_dist, x_seg_start + dx_flight_vec * z_shot_dist, y_seg_start + dy_flight_vec * z_shot_dist);
        LINE_START_FUNCTION;
      #endif
    }
 
  //return
  } else if (z_stage == Z_SEG) {
    nav_route_xy(x_seg_start, y_seg_start, x_seg_end, y_seg_end);
    if (nav_approaching_xy(x_seg_end, y_seg_end, x_seg_start, y_seg_start, 0)) {
     
      // calculate the rest of the points for the next fly-over
      x_ret_start = x_ret_start + dx_sweep_width;
      y_ret_start = y_ret_start + dy_sweep_width;
      x_ret_end = x_ret_end + dx_sweep_width;
      y_ret_end = y_ret_end + dy_sweep_width;
      x_turn_center1 = (x_seg_end + x_ret_start)/2;
      y_turn_center1 = (y_seg_end + y_ret_start)/2;
      x_turn_center2 = (x_seg_start + x_ret_end + dx_sweep_width) / 2;
      y_turn_center2 = (y_seg_start + y_ret_end + dy_sweep_width) / 2;
 
      z_stage = Z_TURN1;
      nav_init_stage();
      #ifdef DIGITAL_CAM
        //dc_stop();
        LINE_STOP_FUNCTION;
      #endif
    }
  }
  if (counter >= laps) {
    #ifdef DIGITAL_CAM
    LINE_STOP_FUNCTION;
    #endif
    return FALSE;
  }
  else {
    return TRUE;
  }
}



From: Felix Ruess
Subject: Re: [Paparazzi-devel] Problem compiling with a custom written subroutine (zamboni survey)
Date: Sun, 10 Mar 2013 17:37:40 +0100

Hi Jorn,

What compile error do you get? Still this one?
/home/jorn/paparazzi/var/My_Lisa_Skywalker/ap/subsystems/nav.o: In function `auto_nav':
/home/jorn/paparazzi/var/My_Lisa_Skywalker/generated/flight_plan.h:191: undefined reference to `init_zamboni_survey'
/home/jorn/paparazzi/var/My_Lisa_Skywalker/generated/flight_plan.h:195: undefined reference to `zamboni_survey'

Did you declare those two functions in your subsystems/navigation/zamboni_survey.h file?

Cheers, Felix

On Sun, Mar 10, 2013 at 12:49 AM, Jorn Anke <address@hidden> wrote:

What I did try was (like before) to add; $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/zamboni_survey.c
to the navigation_extra.makefile  :
__________________
# Hey Emacs, this is a -*- makefile -*-

# standard and extra fixed wing navigation

#add these to all targets

$(TARGET).CFLAGS += -DNAV
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/nav.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/common_flight_plan.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/traffic_info.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/nav_survey_rectangle.c $(SRC_SUBSYSTEMS)/navigation/nav_line.c

$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/nav_cube.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/discsurvey.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/OSAMNav.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/snav.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/spiral.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/poly_survey_adv.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/gls.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/border_line.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/zamboni_survey.c
# $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/lawnmover_survey_gt.c
__________________

Then, to add a separate makefile, in paparazzi/conf/firmwares/subsystems/  directory:
__________________
# Hey Emacs, this is a -*- makefile -*-

# standard and extra fixed wing navigation

#add these to all targets

$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/zamboni_survey.c
_________________

Airframe file starts like this:
_________________
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">

<flight_plan alt="310" ground_alt="185" lat0="59.81" lon0="10.3578" max_dist_from_home="550" name="Test zamboni" security_height="60" qfu="110">
  <header>
#include "subsystems/navigation/zamboni_survey.h"
#include "subsystems/datalink/datalink.h"
/*
#ifndef VARDECLARED
#define VARDECLARED
float varsweepsize=110;
#endif
*/
#include "modules/digital_cam/dc.h"
//#define LINE_START_FUNCTION dc_autoshoot = DC_AUTOSHOOT_DISTANCE;
//#define LINE_START_FUNCTION dc_Survey(dc_gps_dist);
#define LINE_START_FUNCTION dc_Survey(40);
#define LINE_STOP_FUNCTION dc_autoshoot = DC_AUTOSHOOT_STOP;
</header>
_________________


Cheers, Jorn

_______________________________________________
Paparazzi-devel mailing list
address@hidden
https://lists.nongnu.org/mailman/listinfo/paparazzi-devel



reply via email to

[Prev in Thread] Current Thread [Next in Thread]