Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

Implement classic GVF for rotorcrafts (#3327)

Browse Source 
* [modules] implemented GVF for rotorcrafts

* Note about Crazyradio communication with Crazyflies
This commit is contained in:
Angel-HF
2024-07-12 17:44:01 +02:00
committed by GitHub
parent 72fe3fb95a
commit e089fd48f7
8 changed files with 374 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/conf_example.xml
+11
View File
@@ -109,6 +109,17 @@
settings_modules="modules/air_data.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_rotorcraft.xml modules/stabilization_indi_simple.xml" settings_modules="modules/air_data.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_rotorcraft.xml modules/stabilization_indi_simple.xml"
gui_color="red" gui_color="red"
/> />
<aircraft
name="bebop2_gvf"
ac_id="204"
airframe="airframes/examples/bebop2_gvf.xml"
radio="radios/dummy.xml"
telemetry="telemetry/GVF/gvf_default_rotorcraft.xml"
flight_plan="flight_plans/demo_gvf_rotorcraft.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml"
settings_modules="modules/air_data.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gvf_module.xml~GVF~ modules/imu_common.xml modules/ins_ekf2.xml modules/nav_rotorcraft.xml modules/stabilization_indi_simple.xml"
gui_color="red"
/>
<aircraft <aircraft
name="bebop2_opticflow" name="bebop2_opticflow"
ac_id="3" ac_id="3"
conf/flight_plans/demo_gvf_rotorcraft.xml
+88
View File
@@ -0,0 +1,88 @@
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
<flight_plan alt="720.5" ground_alt="720" lat0="37.296981" lon0="-3.683133" max_dist_from_home="200" name="Rotorcraft GVF" security_height="0.1">
<waypoints>
<waypoint name="HOME" x="0.0" y="0.0"/>
<waypoint name="STDBY" x="0.1" y="0.8"/>
<waypoint name="S1_START" x="0.0" y="1.0"/>
<waypoint name="S1_END" x="0.0" y="5.0"/>
<waypoint name="S2_START" x="2.0" y="1.0"/>
<waypoint name="S2_END" x="2.0" y="5.0"/>
<waypoint name="TD" x="-1.0" y="1.5"/>
</waypoints>
<variables>
<variable init="0.1" var="fp_throttle"/>
<variable var="desired_heading" init="0.0f" type="float" min="0." max="10." step="0.1"/>
<variable var="nominal_alt" init="0.5" type="float" min="0." max="10." step="0.1"/>
</variables>
<modules>
<module name="gvf_module"/>
<!-- module name="gvf_parametric"/--> <!-- Not implemented yet -->
</modules>
<blocks>
<block name="FPInit">
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid()"/>
<while cond="TRUE"/>
</block>
<block name="Holding point">
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine" strip_button="Start Engine" strip_icon="resurrect.png" group="home">
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Take off" strip_button="Takeoff" strip_icon="takeoff.png" group="home">
<call_once fun="NavVerticalAltitudeMode(nominal_alt, 0.0)"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png" group="home">
<stay wp="STDBY"/>
</block>
<block name="GVF Segment 1">
<call fun="gvf_segment_loop_wp1_wp2(WP_S1_START, WP_S1_END, gvf_segment_par.d1, gvf_segment_par.d2)"/>
</block>
<block name="GVF Segment 2">
<call fun="gvf_segment_loop_wp1_wp2(WP_S2_START, WP_S2_END, gvf_segment_par.d1, gvf_segment_par.d2)"/>
</block>
<block name="GVF 25m Circle">
<call fun="gvf_ellipse_wp(WP_TD, 25, 25, gvf_ellipse_par.alpha)"/>
</block>
<!-- Function found in ./sw/airborne/modules/guidance/gfv/gvf.h -->
<block name="GVF Ellipse">
<call fun="gvf_ellipse_wp(WP_TD, gvf_ellipse_par.a, gvf_ellipse_par.b, gvf_ellipse_par.alpha)"/>
</block>
<block name="Land here" key="l" strip_button="Land Here" strip_icon="land-right.png">
<call_once fun="NavSetWaypointHere(WP_TD)"/>
</block>
<block name="Land">
<go wp="TD"/>
</block>
<block name="Land Target" strip_button="Land Target" group="target">
<exception cond="!nav_is_in_flight()" deroute="Kill Engine"/>
<stay climb="nav.descend_vspeed" vmode="climb" wp="TD"/>
</block>
<block name="Kill Engine" key="k">
<call_once fun="NavKillThrottle()"/>
<while cond="1"/>
</block>
</blocks>
</flight_plan>
conf/modules/gvf_module.xml
+4 -2
View File
@@ -33,12 +33,14 @@ For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_
<dl_settings NAME="GVF"> <dl_settings NAME="GVF">
<dl_settings NAME="Control"> <dl_settings NAME="Control">
<dl_setting MAX="1" MIN="-1" STEP="2" VAR="gvf_control.s" shortname = "direction"/> <dl_setting MAX="1" MIN="-1" STEP="2" VAR="gvf_control.s" shortname = "direction"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="gvf_control.align" shortname = "(ROTORCRAFT) align with trajectory"/>
<dl_setting MAX="20" MIN="0.0" STEP="0.2" VAR="gvf_control.speed" shortname = "(ROTORCRAFT) speed"/>
</dl_settings> </dl_settings>
<dl_settings NAME="Ellipse"> <dl_settings NAME="Ellipse">
<dl_setting MAX="5" MIN="0.0" STEP="0.01" VAR="gvf_ellipse_par.ke" shortname="ell_ke" param="GVF_ELLIPSE_KE"/> <dl_setting MAX="5" MIN="0.0" STEP="0.01" VAR="gvf_ellipse_par.ke" shortname="ell_ke" param="GVF_ELLIPSE_KE"/>
<dl_setting MAX="5" MIN="0.0" STEP="0.01" VAR="gvf_ellipse_par.kn" shortname="ell_kn" param="GVF_ELLIPSE_KN"/> <dl_setting MAX="5" MIN="0.0" STEP="0.01" VAR="gvf_ellipse_par.kn" shortname="ell_kn" param="GVF_ELLIPSE_KN"/>
<dl_setting MAX="150" MIN="0.0" STEP="10" VAR="gvf_ellipse_par.a" shortname="ell_a" param="GVF_ELLIPSE_A"/> <dl_setting MAX="150" MIN="0.0" STEP="5" VAR="gvf_ellipse_par.a" shortname="ell_a" param="GVF_ELLIPSE_A"/>
<dl_setting MAX="150" MIN="0.0" STEP="10" VAR="gvf_ellipse_par.b" shortname="ell_b" param="GVF_ELLIPSE_B"/> <dl_setting MAX="150" MIN="0.0" STEP="5" VAR="gvf_ellipse_par.b" shortname="ell_b" param="GVF_ELLIPSE_B"/>
<dl_setting MAX="90" MIN="-90" STEP="1" VAR="gvf_ellipse_par.alpha" shortname="ell_alpha" param="GVF_ELLIPSE_ALPHA"/> <dl_setting MAX="90" MIN="-90" STEP="1" VAR="gvf_ellipse_par.alpha" shortname="ell_alpha" param="GVF_ELLIPSE_ALPHA"/>
</dl_settings> </dl_settings>
<dl_settings NAME="Line"> <dl_settings NAME="Line">
conf/modules/nav_rotorcraft.xml
+1 -1
View File
@@ -9,7 +9,7 @@
<settings> <settings>
<dl_settings> <dl_settings>
<dl_settings NAME="NAV"> <dl_settings NAME="NAV">
<dl_setting var="flight_altitude" MIN="0" STEP="0.1" MAX="400" module="navigation" unit="m" handler="SetFlightAltitude"/> <dl_setting var="flight_altitude" MIN="0" STEP="0.1" MAX="999" module="navigation" unit="m" handler="SetFlightAltitude"/>
<dl_setting var="nav.heading" MIN="0" STEP="0.1" MAX="360" unit="rad" alt_unit="deg"/> <dl_setting var="nav.heading" MIN="0" STEP="0.1" MAX="360" unit="rad" alt_unit="deg"/>
<dl_setting var="nav.radius" MIN="-50" STEP="0.1" MAX="50" unit="m"/> <dl_setting var="nav.radius" MIN="-50" STEP="0.1" MAX="50" unit="m"/>
<dl_setting var="nav.climb_vspeed" MIN="0" STEP="0.1" MAX="10.0" unit="m/s" param="NAV_CLIMB_VSPEED"/> <dl_setting var="nav.climb_vspeed" MIN="0" STEP="0.1" MAX="10.0" unit="m/s" param="NAV_CLIMB_VSPEED"/>
conf/telemetry/GVF/gvf_default_rotorcraft.xml
+216
View File
@@ -0,0 +1,216 @@
<?xml version="1.0"?>
<!DOCTYPE telemetry SYSTEM "../telemetry.dtd">
<telemetry>
<process name="Main">
<mode name="default" key_press="d">
<message name="AUTOPILOT_VERSION" period="11.1"/>
<message name="DL_VALUE" period="1.1"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_FP" period="0.25"/>
<message name="ALIVE" period="2.1"/>
<message name="INS_REF" period="5.1"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/>
<message name="ROTORCRAFT_CAM" period="1."/>
<message name="GPS_INT" period=".25"/>
<message name="INS" period=".25"/>
<message name="GVF" period="3."/>
<message name="GVF_PARAMETRIC" period="0.5"/>
<message name="I2C_ERRORS" period="4.1"/>
<message name="UART_ERRORS" period="3.1"/>
<message name="SUPERBITRF" period="3"/>
<message name="ENERGY" period="2.5"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="STATE_FILTER_STATUS" period="3.2"/>
<message name="AIR_DATA" period="1.3"/>
<message name="SURVEY" period="2.5"/>
<message name="OPTIC_FLOW_EST" period="0.05"/>
<message name="VECTORNAV_INFO" period="0.5"/>
<message name="OPTICAL_FLOW_HOVER" period="0.05"/>
<message name="VISUALTARGET" period="0.10"/>
<message name="VISION_POSITION_ESTIMATE" period="0.1"/>
<message name="DIVERGENCE" period="0.05"/>
<message name="DRAGSPEED" period="0.02"/>
<message name="LOGGER_STATUS" period="5.1"/>
<message name="LIDAR" period="1.2"/>
<message name="INS_EKF2" period=".25"/>
<message name="WIND_INFO_RET" period="1."/>
<message name="AHRS_REF_QUAT" period="0.5"/>
<message name="STAB_ATTITUDE" period=".25"/>
<message name="EFF_MAT_G" period="2.0"/>
</mode>
<mode name="ppm">
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="ROTORCRAFT_CMD" period=".05"/>
<message name="PPM" period="0.5"/>
<message name="RC" period="0.5"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.5"/>
<message name="ROTORCRAFT_STATUS" period="1"/>
<message name="BEBOP_ACTUATORS" period="0.2"/>
</mode>
<mode name="raw_sensors">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="IMU_ACCEL_RAW" period=".05"/>
<message name="IMU_GYRO_RAW" period=".05"/>
<message name="IMU_MAG_RAW" period=".05"/>
<message name="BARO_RAW" period=".1"/>
<message name="IMU_MAG_CURRENT_CALIBRATION" period=".05"/>
<message name="ARDRONE_NAVDATA" period=".05"/>
<message name="AIRSPEED_RAW" period="0.1"/>
</mode>
<mode name="scaled_sensors">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="IMU_GYRO_SCALED" period=".075"/>
<message name="IMU_ACCEL_SCALED" period=".075"/>
<message name="IMU_MAG_SCALED" period=".1"/>
</mode>
<mode name="ahrs">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="FILTER_ALIGNER" period="2.2"/>
<message name="FILTER" period=".5"/>
<message name="GEO_MAG" period="5."/>
<message name="AHRS_GYRO_BIAS_INT" period="0.08"/>
<message name="AHRS_QUAT_INT" period=".25"/>
<message name="AHRS_EULER_INT" period=".1"/>
<message name="AHRS_EULER" period=".1"/>
<!-- <message name="AHRS_RMAT_INT" period=".5"/> -->
</mode>
<mode name="rate_loop">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="RATE_LOOP" period=".02"/>
</mode>
<mode name="attitude_setpoint_viz" key_press="v">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.1"/>
<message name="AHRS_REF_QUAT" period="0.05"/>
</mode>
<mode name="attitude_loop" key_press="a">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="STAB_ATTITUDE_INT" period=".03"/>
<message name="STAB_ATTITUDE_REF_INT" period=".03"/>
<message name="STAB_ATTITUDE_FLOAT" period=".03"/>
<message name="STAB_ATTITUDE_REF_FLOAT" period=".03"/>
<message name="STAB_ATTITUDE" period=".25"/>
<message name="EFF_MAT_G" period="2.0"/>
</mode>
<mode name="vert_loop" key_press="v">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="VFF" period=".05"/>
<message name="VFF_EXTENDED" period=".05"/>
<message name="VERT_LOOP" period=".05"/>
<message name="INS_Z" period=".05"/>
<message name="INS" period=".11"/>
<message name="INS_REF" period="5.1"/>
</mode>
<mode name="vel_guidance" key_press="q">
<message name="ALIVE" period="0.9"/>
<message name="HYBRID_GUIDANCE" period="0.062"/>
<message name="GUIDANCE_INDI_HYBRID" period="0.1"/>
<!--<message name="STAB_ATTITUDE_REF" period="0.4"/>-->
<message name="ROTORCRAFT_FP" period="0.8"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="INS_REF" period="5.1"/>
<message name="WP_MOVED" period="1.3"/>
<message name="GPS_INT" period="1.0"/>
<message name="INS" period="1.0"/>
</mode>
<mode name="h_loop" key_press="h">
<message name="ALIVE" period="0.9"/>
<message name="HOVER_LOOP" period="0.062"/>
<message name="GUIDANCE_H_REF_INT" period="0.062"/>
<message name="STAB_ATTITUDE_INT" period="0.4"/>
<message name="STAB_ATTITUDE_FLOAT" period="0.4"/>
<!--<message name="STAB_ATTITUDE_REF_INT" period="0.4"/>-->
<message name="ROTORCRAFT_FP" period="0.8"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="INS_REF" period="5.1"/>
<!-- HFF messages are only sent if USE_HFF -->
<message name="HFF" period=".05"/>
<message name="HFF_GPS" period=".03"/>
<message name="HFF_DBG" period=".2"/>
</mode>
<mode name="aligner">
<message name="ALIVE" period="0.9"/>
<message name="FILTER_ALIGNER" period="0.02"/>
</mode>
<mode name="tune_hover">
<message name="DL_VALUE" period="1.1"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ALIVE" period="2.1"/>
<message name="GUIDANCE_H_INT" period="0.05"/>
<message name="ROTORCRAFT_TUNE_HOVER" period=".1"/>
<!-- <message name="GPS_INT" period=".20"/> -->
<!--<message name="INS2" period=".05"/>
<message name="INS3" period=".20"/>-->
<message name="INS_REF" period="5.1"/>
</mode>
<mode name="RTCM3" >
<message name="GPS_RXMRTCM" period="1"/>
<message name="GPS_INT" period=".25"/>
<message name="GPS_RTK" period="1"/>
</mode>
</process>
<process name="FlightRecorder">
<mode name="default">
<message name="AUTOPILOT_VERSION" period="11.1"/>
<message name="DL_VALUE" period="1.1"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="ROTORCRAFT_FP" period="0.25"/>
<message name="INS_REF" period="5.1"/>
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/>
<message name="GPS_INT" period=".1"/>
<message name="INS" period=".1"/>
<message name="I2C_ERRORS" period="4.1"/>
<message name="UART_ERRORS" period="3.1"/>
<message name="ENERGY" period="2.5"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="STATE_FILTER_STATUS" period="3.2"/>
<message name="AIR_DATA" period="1.3"/>
<message name="SURVEY" period="2.5"/>
<message name="IMU_GYRO_SCALED" period=".075"/>
<message name="IMU_ACCEL_SCALED" period=".075"/>
<message name="IMU_MAG_SCALED" period=".2"/>
<message name="LIDAR" period="0.05"/>
<message name="ESC" period="0.05"/>
<message name="STAB_ATTITUDE" period=".1"/>
</mode>
</process>
</telemetry>
sw/airborne/modules/guidance/gvf/gvf.c
+39 -6
View File
@@ -118,6 +118,8 @@ void gvf_init(void)
gvf_control.ke = 1; gvf_control.ke = 1;
gvf_control.kn = 1; gvf_control.kn = 1;
gvf_control.s = 1; gvf_control.s = 1;
gvf_control.speed = 1.0; // Rotorcraft only (for now)
gvf_control.align = false; // Rotorcraft only
gvf_trajectory.type = NONE; gvf_trajectory.type = NONE;
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
@@ -177,18 +179,18 @@ void gvf_control_2D(float ke, float kn, float e,
float md_dot_x = md_y * md_dot_const; float md_dot_x = md_y * md_dot_const;
float md_dot_y = -md_x * md_dot_const; float md_dot_y = -md_x * md_dot_const;
#if defined(ROTORCRAFT_FIRMWARE)
#ifdef ROTORCRAFT_FIRMWARE // Use accel based control. Not recommended as of current implementation
#if defined(GVF_ROTORCRAFT_USE_ACCEL)
// Set nav for command // Set nav for command
// Use parameter kn as the speed command // Use parameter kn as the speed command
nav.speed.x = md_x * kn; nav.speed.x = md_x * kn;
nav.speed.y = md_y * kn; nav.speed.y = md_y * kn;
// Acceleration induced by the field with speed set to kn (!WIP!) // Acceleration induced by the field with speed set to kn (!WIP!)
#warning "Using GVF for rotorcraft is still experimental, proceed with caution" #warning "Using GVF for rotorcraft is still experimental, proceed with caution"
float n_norm = sqrtf(nx*nx+ny*ny); float n_norm = sqrtf(nx*nx+ny*ny);
float hess_px_dot = px_dot * H11 + py_dot * H12; float hess_px_dot = px_dot * H11 + py_dot * H12;
float hess_py_dot = px_dot * H21 + py_dot * H22; float hess_py_dot = px_dot * H21 + py_dot * H22;
@@ -205,12 +207,28 @@ void gvf_control_2D(float ke, float kn, float e,
float speed_cmd_x = kn*tx / n_norm - ke * e * nx / (n_norm); float speed_cmd_x = kn*tx / n_norm - ke * e * nx / (n_norm);
float speed_cmd_y = kn*ty / n_norm - ke * e * ny / (n_norm); float speed_cmd_y = kn*ty / n_norm - ke * e * ny / (n_norm);
// TODO don't change nav struct directly // TODO: don't change nav struct directly
nav.accel.x = accel_cmd_x + (speed_cmd_x - px_dot); nav.accel.x = accel_cmd_x + (speed_cmd_x - px_dot);
nav.accel.y = accel_cmd_y + (speed_cmd_y - py_dot); nav.accel.y = accel_cmd_y + (speed_cmd_y - py_dot);
nav.heading = atan2f(md_x,md_y); nav.heading = atan2f(md_x,md_y);
#else #else // SPEED_BASED_GVF
nav.setpoint_mode = NAV_SETPOINT_MODE_SPEED;
// Speed-based control, acceleration based control not implemented yet
nav.speed.x = gvf_control.speed * md_x;
nav.speed.y = gvf_control.speed * md_y;
// Optionally align heading with trajectory
if (gvf_control.align)
{
nav.heading = atan2f(md_x, md_y);
}
#endif
#else // FIXEDWING / ROVER FIRMWARE
float omega_d = -(md_dot_x * md_y - md_dot_y * md_x); float omega_d = -(md_dot_x * md_y - md_dot_y * md_x);
@@ -230,6 +248,21 @@ void gvf_control_2D(float ke, float kn, float e,
#endif #endif
} }
// BEGIN ROTORCRAFT
void gvf_set_speed(float speed)
{
if (speed < 0.0) speed = 0.0;
gvf_control.speed = speed;
}
void gvf_set_align(bool align)
{
gvf_control.align = align;
}
// END ROTORCRAFT
void gvf_set_direction(int8_t s) void gvf_set_direction(int8_t s)
{ {
gvf_control.s = s; gvf_control.s = s;
sw/airborne/modules/guidance/gvf/gvf.h
+9 -4
View File
@@ -42,14 +42,19 @@
* @param ke Gain defining how agressive is the vector field * @param ke Gain defining how agressive is the vector field
* @param kn Gain for making converge the vehile to the vector field * @param kn Gain for making converge the vehile to the vector field
* @param error Error signal. It does not have any specific units. It depends on how the trajectory has been implemented. Check the specific wiki entry for each trajectory. * @param error Error signal. It does not have any specific units. It depends on how the trajectory has been implemented. Check the specific wiki entry for each trajectory.
* @param omega Angular velocity of the vehicle
* @param speed Speed of the vehicle. It is only used in rotorcrafts for now.
* @param s Defines the direction to be tracked. Its meaning depends on the trajectory and its implementation. Check the wiki entry of the GVF. It takes the values -1 or 1. * @param s Defines the direction to be tracked. Its meaning depends on the trajectory and its implementation. Check the wiki entry of the GVF. It takes the values -1 or 1.
* @param align Align the vehicle with the direction of the vector field. Can only used in rotorcrafts.
*/ */
typedef struct { typedef struct {
float ke; float ke;
float kn; float kn;
float error; float error;
float omega; float omega;
float speed;
int8_t s; int8_t s;
bool align;
} gvf_con; } gvf_con;
extern gvf_con gvf_control; extern gvf_con gvf_control;
@@ -113,9 +118,11 @@ struct gvf_Hess {
extern void gvf_init(void); extern void gvf_init(void);
void gvf_control_2D(float ke, float kn, float e, void gvf_control_2D(float ke, float kn, float e,
struct gvf_grad *, struct gvf_Hess *); struct gvf_grad *, struct gvf_Hess *);
extern void gvf_set_speed(float speed); // Rotorcraft only (for now)
extern void gvf_set_align(bool align); // Rotorcraft only
extern void gvf_set_direction(int8_t s); extern void gvf_set_direction(int8_t s);
// Straigh line // Straight line
extern bool gvf_line_XY_heading(float x, float y, float heading); extern bool gvf_line_XY_heading(float x, float y, float heading);
extern bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2); extern bool gvf_line_XY1_XY2(float x1, float y1, float x2, float y2);
extern bool gvf_line_wp1_wp2(uint8_t wp1, uint8_t wp2); extern bool gvf_line_wp1_wp2(uint8_t wp1, uint8_t wp2);
@@ -125,7 +132,6 @@ extern bool gvf_segment_XY1_XY2(float x1, float y1, float x2, float y2);
extern bool gvf_segment_wp1_wp2(uint8_t wp1, uint8_t wp2); extern bool gvf_segment_wp1_wp2(uint8_t wp1, uint8_t wp2);
extern bool gvf_line_wp_heading(uint8_t wp, float heading); extern bool gvf_line_wp_heading(uint8_t wp, float heading);
// Ellipse // Ellipse
extern bool gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha); extern bool gvf_ellipse_wp(uint8_t wp, float a, float b, float alpha);
extern bool gvf_ellipse_XY(float x, float y, float a, float b, float alpha); extern bool gvf_ellipse_XY(float x, float y, float a, float b, float alpha);
@@ -137,5 +143,4 @@ extern bool gvf_sin_wp1_wp2(uint8_t wp1, uint8_t wp2, float w, float off,
extern bool gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off, extern bool gvf_sin_wp_alpha(uint8_t wp, float alpha, float w, float off,
float A); float A);
#endif // GVF_H
#endif // GVF_H
sw/ground_segment/python/bitcraze/crazyradio2ivy.py
+6 -4
View File
@@ -5,6 +5,11 @@ with support of PPRZLINK messages
Requires 'pip3 install cflib' Requires 'pip3 install cflib'
IMPORTANT:
To make this script work, you need to have the Crazyflie firmware 2019.09.
Some other firmwares may work, but the 2019.09 is the one that has been tested.
2024 firmwares onwards DO NOT work for sure.
As the ESB protocol works using PTX and PRX (Primary Transmitter/Reciever) As the ESB protocol works using PTX and PRX (Primary Transmitter/Reciever)
modes. Thus, data is only recieved as a response to a sent packet. modes. Thus, data is only recieved as a response to a sent packet.
So, we need to constantly poll the receivers for bidirectional communication. So, we need to constantly poll the receivers for bidirectional communication.
@@ -34,12 +39,10 @@ import pprzlink.messages_xml_map as messages_xml_map
CRTP_PORT_PPRZLINK = 9 CRTP_PORT_PPRZLINK = 9
# Only output errors from the logging framework # Only output errors from the logging framework
#logging.basicConfig(level=logging.DEBUG) #logging.basicConfig(level=logging.DEBUG)
logging.basicConfig(level=logging.ERROR) logging.basicConfig(level=logging.ERROR)
class RadioBridge: class RadioBridge:
def __init__(self, link_uri, msg_class='telemetry', verbose=False): def __init__(self, link_uri, msg_class='telemetry', verbose=False):
""" Initialize and run with the specified link_uri """ """ Initialize and run with the specified link_uri """
@@ -173,5 +176,4 @@ if __name__ == '__main__':
bridge.shutdown() bridge.shutdown()
time.sleep(1) time.sleep(1)
sys.exit() sys.exit()