Skip to content

Commit

Permalink
Logo600 flight on Kalamos in belgium
Browse files Browse the repository at this point in the history
  • Loading branch information
@kevindehecker
kevindehecker committed Jul 7, 2016
1 parent b2c7f59 commit 828e0b5
Show file tree
Hide file tree
Showing 5 changed files with 108 additions and 5 deletions.
3 changes: 2 additions & 1 deletion conf/airframes/TUDELFT/tudelft_logo600.xml
View file
Expand Up @@ -223,6 +223,7 @@
<define name="HOVER_KI" value="15"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.62"/>
<define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
<define name="REF_MAX_ZDD" value="(9.81*0.05)"/>
</section>

<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
Expand All @@ -246,7 +247,7 @@
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_RC_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO2" value="AP_MODE_HOVER_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
</section>

<section name="BAT">
Expand Down
94 changes: 94 additions & 0 deletions conf/flight_plans/TUDELFT/logo600_belgie.xml
View file
@@ -0,0 +1,94 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">

<flight_plan alt="180" ground_alt="150" lat0="51 24 20.2212" lon0="4 27 33.192" max_dist_from_home="150" name="Logo600_Belgie" security_height="2">
<header>
#include "autopilot.h"
</header>
<waypoints>
<waypoint name="HOME" x="0.0" y="0.0"/>
<waypoint name="CLIMB" x="0.0" y="5.0"/>
<waypoint name="STDBY" x="-2.0" y="-5.0"/>
<waypoint name="p1" x="3.6" y="-13.9"/>
<waypoint name="p2" x="27.5" y="-48.2"/>
<waypoint name="p3" x="16.7" y="-19.6"/>
<waypoint name="p4" x="13.7" y="-40.7"/>
<waypoint name="CAM" x="-20" y="-50" height="2."/>
<waypoint name="TD" x="5.6" y="-10.9"/>
<waypoint name="KALAMOS" x="0.0" y="120.0"/>
</waypoints>
<blocks>
<block name="Wait GPS">
<call fun="NavKillThrottle()"/>
<while cond="!GpsFixValid()"/>
</block>
<block name="Geo init">
<while cond="LessThan(NavBlockTime(), 10)"/>
<call fun="NavSetGroundReferenceHere()"/>
<!--<call fun="NavSetAltitudeReferenceHere()"/>-->
</block>
<block name="Holding point">
<call fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>
</block>
<block name="Start Engine">
<call fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<exception cond="stateGetPositionEnu_f()->z > 2.0" deroute="Standby"/>
<call fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay vmode="climb" climb="nav_climb_vspeed" wp="CLIMB"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<stay wp="STDBY"/>
</block>
<block name="stay_kalamos">
<stay wp="KALAMOS"/>
</block>
<block name="go_p2">
<call fun="nav_set_heading_deg(90)"/>
<go wp="p2"/>
<deroute block="stay_kalamos"/>
</block>
<block name="line_p1_p2">
<go from="p1" hmode="route" wp="p2"/>
<stay wp="p2" until="stage_time>10"/>
<go from="p2" hmode="route" wp="p1"/>
<deroute block="stay_kalamos"/>
</block>
<block name="route">
<go from="p1" hmode="route" wp="p3"/>
<go from="p3" hmode="route" wp="p4"/>
<go from="p4" hmode="route" wp="p1"/>
<deroute block="stay_kalamos"/>
</block>
<block name="Oval">
<oval p1="p1" p2="p2" radius="-1"/>
</block>
<block name="test yaw">
<go wp="p1"/>
<for var="i" from="1" to="16">
<heading alt="WaypointAlt(WP_p1)" course="90 * $i" until="stage_time > 3"/>
</for>
<deroute block="Standby"/>
</block>
<block name="circle CAM" pre_call="nav_set_heading_towards_waypoint(WP_CAM)">
<circle radius="nav_radius" wp="CAM"/>
</block>
<block name="land here" strip_button="Land Here" strip_icon="land-right.png">
<call fun="NavSetWaypointHere(WP_TD)"/>
</block>
<block name="land">
<go wp="TD"/>
</block>
<block name="flare">
<exception cond="NavDetectGround()" deroute="Holding point"/>
<exception cond="!nav_is_in_flight()" deroute="landed"/>
<call fun="NavStartDetectGround()"/>
<stay climb="nav_descend_vspeed" vmode="climb" wp="TD"/>
</block>
<block name="landed">
<attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>
</block>
</blocks>
</flight_plan>
7 changes: 7 additions & 0 deletions conf/modules/kalamos_uart.xml
View file
Expand Up @@ -6,6 +6,13 @@
<configure name="KALAMOS_PORT" value="UART5" description="select which uart it is connected to"/>
<configure name="KALAMOS_BAUD" value="B115200" description="set the baudrate of the uart"/>
</doc>
<settings>
<dl_settings NAME="kalamos">
<dl_settings NAME="Kalamos">
<dl_setting module="sensors/kalamos_uart" var="kalamos_target_height" min="1" max="100" step="1"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="kalamos_uart.h"/>
</header>
Expand Down
7 changes: 4 additions & 3 deletions sw/airborne/modules/sensors/kalamos_uart.c
View file
Expand Up @@ -45,6 +45,7 @@ static struct kalamos_t kalamos = {
static uint8_t mp_msg_buf[128] __attribute__((aligned)); ///< The message buffer for the Kalamos

struct Kalamos2PPRZPackage k2p_package;
float kalamos_target_height;

#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
Expand Down Expand Up @@ -137,13 +138,13 @@ void kalamos_periodic() {
1, sizeof(struct PPRZ2KalamosPackage), (unsigned char *)(&p2k_package));

//get pprz height
// calculate it so that sonar is 10m high
// calculate it so that sonar is x meter high
//set waypoint to that number

struct EnuCoor_f *pos = stateGetPositionEnu_f();

float target_kalamos_height = 10;
float diff = (target_kalamos_height - k2p_package.height);
//float target_kalamos_height = 30;
float diff = (kalamos_target_height - k2p_package.height);
float pprzheight = pos->z + diff;

waypoint_set_alt(WP_KALAMOS,pprzheight );
Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/modules/sensors/kalamos_uart.h
View file
Expand Up @@ -59,7 +59,7 @@ struct PPRZ2KalamosPackage {
float theta;
};


extern float kalamos_target_height;

extern void kalamos_init(void);
extern void kalamos_event(void);
Expand Down

0 comments on commit 828e0b5

Please sign in to comment.