Skip to content

Commit

Permalink
+hover with pix4flow
Browse files Browse the repository at this point in the history
  • Loading branch information
@rmeertens
rmeertens committed Oct 29, 2015
1 parent 72cf212 commit a39d163
Show file tree
Hide file tree
Showing 5 changed files with 267 additions and 10 deletions.
10 changes: 8 additions & 2 deletions conf/airframes/RM/rm_ardrone2.xml
View file
Expand Up @@ -33,7 +33,13 @@

<modules main_freq="512">
<load name="bat_voltage_ardrone2.xml"/>
<load name="uartrotation.xml"/>
<!--load name="uartrotation.xml"/-->
<load name="opticflow_hover.xml"/>
<load name="mavlink_decoder.xml">
<configure name="MAVLINK_PORT" value="UART1"/>
<configure name="MAVLINK_BAUD" value="B115200"/>
</load>
<load name="px4flow.xml"/>
</modules>

<commands>
Expand Down Expand Up @@ -223,7 +229,7 @@
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<define name="MODE_AUTO2" value="AP_MODE_MODULE"/>

<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
</section>
Expand Down
241 changes: 241 additions & 0 deletions conf/airframes/examples/ardrone2_px4flow_hover.xml
View file
@@ -0,0 +1,241 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<!-- RM airframe with INDI and video logging-->
<airframe name="ardrone2_indi">
<firmware name="rotorcraft">
<target name="ap" board="ardrone2">
<define name="FAILSAFE_DESCENT_SPEED" value="0.5"/>
<configure name="USE_MAGNETOMETER" value="1"/>
<define name="AUTOPILOT_DISABLE_AHRS_KILL"/>
</target>

<target name="nps" board="pc">
<subsystem name="fdm" type="jsbsim"/>
</target>

<define name="USE_SONAR" value="TRUE"/>

<!-- Subsystem section -->
<subsystem name="telemetry" type="transparent_udp"/>
<subsystem name="radio_control" type="datalink"/>
<subsystem name="motor_mixing"/>
<subsystem name="actuators" type="ardrone2"/>
<subsystem name="imu" type="ardrone2"/>
<!-- gps: "ublox" or change to "sirf" for usage with parrot flight recorder -->
<subsystem name="gps" type="datalink"/>
<subsystem name="stabilization" type="indi"/>
<subsystem name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="TRUE"/>
<define name="AHRS_USE_GPS_HEADING" value="FALSE"/>
</subsystem>
<subsystem name="ins" type="extended"/>
</firmware>

<modules main_freq="512">
<load name="bat_voltage_ardrone2.xml"/>
<load name="opticflow_hover.xml"/>
<load name="mavlink_decoder.xml">
<configure name="MAVLINK_PORT" value="UART1"/>
<configure name="MAVLINK_BAUD" value="B115200"/>
</load>
<load name="px4flow.xml"/>
</modules>

<commands>
<axis name="PITCH" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="3000"/>
</commands>

<servos driver="Default">
<servo name="TOP_LEFT" no="0" min="0" neutral="1" max="500"/>
<servo name="TOP_RIGHT" no="1" min="0" neutral="1" max="500"/>
<servo name="BOTTOM_RIGHT" no="2" min="0" neutral="1" max="500"/>
<servo name="BOTTOM_LEFT" no="3" min="0" neutral="1" max="500"/>
</servos>

<section name="MIXING" prefix="MOTOR_MIXING_">
<define name="TRIM_ROLL" value="0"/>
<define name="TRIM_PITCH" value="0"/>
<define name="TRIM_YAW" value="0"/>

<!-- Time cross layout (X), with order NW (CW), NE (CCW), SE (CW), SW (CCW) -->
<define name="TYPE" value="QUAD_X"/>
</section>

<command_laws>
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
<set servo="TOP_LEFT" value="motor_mixing.commands[MOTOR_FRONT_LEFT]"/>
<set servo="TOP_RIGHT" value="motor_mixing.commands[MOTOR_FRONT_RIGHT]"/>
<set servo="BOTTOM_RIGHT" value="motor_mixing.commands[MOTOR_BACK_RIGHT]"/>
<set servo="BOTTOM_LEFT" value="motor_mixing.commands[MOTOR_BACK_LEFT]"/>
</command_laws>

<section name="IMU" prefix="IMU_">
<!-- Accelero -->
<define name="ACCEL_X_NEUTRAL" value="2048"/>
<define name="ACCEL_Y_NEUTRAL" value="2048"/>
<define name="ACCEL_Z_NEUTRAL" value="2048"/>

<!-- Magneto calibration -->
<define name="MAG_X_NEUTRAL" value="0"/>
<define name="MAG_Y_NEUTRAL" value="0"/>
<define name="MAG_Z_NEUTRAL" value="-180"/>
<define name="MAG_X_SENS" value="16." integer="16"/>
<define name="MAG_Y_SENS" value="16." integer="16"/>
<define name="MAG_Z_SENS" value="16." integer="16"/>

<!-- Magneto current calibration -->
<define name="MAG_X_CURRENT_COEF" value="0.0"/>
<define name="MAG_Y_CURRENT_COEF" value="0.0"/>
<define name="MAG_Z_CURRENT_COEF" value="0.0"/>

<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section>

<!-- local magnetic field -->
<!-- http://wiki.paparazziuav.org/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
<section name="AHRS" prefix="AHRS_">
<!-- values used if no GPS fix, on 3D fix is update by geo_mag module -->

<!-- Delft -->
<define name="H_X" value="0.3892503"/>
<define name="H_Y" value="0.0017972"/>
<define name="H_Z" value="0.9211303"/>
</section>

<section name="INS" prefix="INS_">
<define name="SONAR_MAX_RANGE" value="3.2"/>
<define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
<!-- Use GPS altitude measurments and set the R gain -->
<!--<define name="USE_GPS_ALT" value="1"/>-->
<!--<define name="VFF_R_GPS" value="0.01"/>-->
</section>

<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<!-- setpoints -->
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="DEADBAND_P" value="20"/>
<define name="DEADBAND_Q" value="20"/>
<define name="DEADBAND_R" value="200"/>
<define name="REF_TAU" value="4"/>

<!-- feedback -->
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>

<define name="IGAIN_P" value="75"/>
<define name="IGAIN_Q" value="75"/>
<define name="IGAIN_R" value="50"/>

<!-- feedforward -->
<define name="DDGAIN_P" value="300"/>
<define name="DDGAIN_Q" value="300"/>
<define name="DDGAIN_R" value="300"/>
</section>

<section name="RC_SETPOINT" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoint limits for attitude stabilization rc flight -->
<define name="SP_MAX_PHI" value="45" unit="deg"/>
<define name="SP_MAX_THETA" value="45" unit="deg"/>
<define name="SP_MAX_R" value="180" unit="deg/s"/>
<define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="250"/>
</section>

<section name="ATTITUDE_REFERENCE" prefix="STABILIZATION_ATTITUDE_">
<!-- attitude reference generation model -->
<define name="REF_OMEGA_P" value="450" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.9"/>
<define name="REF_MAX_P" value="600." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>

<define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.9"/>
<define name="REF_MAX_Q" value="600." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>

<define name="REF_OMEGA_R" value="450" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.9"/>
<define name="REF_MAX_R" value="600." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(8000.)"/>
</section>

<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<!-- control effectiveness -->
<define name="G1_P" value="0.032"/>
<define name="G1_Q" value="0.025"/>
<define name="G1_R" value="0.0032"/>
<define name="G2_R" value="0.16"/>

<!-- reference acceleration for attitude control -->
<define name="REF_ERR_P" value="380.0"/>
<define name="REF_ERR_Q" value="380.0"/>
<define name="REF_ERR_R" value="250.0"/>
<define name="REF_RATE_P" value="21.6"/>
<define name="REF_RATE_Q" value="21.6"/>
<define name="REF_RATE_R" value="21.0"/>

<!-- second order filter parameters -->
<define name="FILT_OMEGA" value="20.0"/>
<define name="FILT_ZETA" value="0.55"/>
<define name="FILT_OMEGA_R" value="20.0"/>
<define name="FILT_ZETA_R" value="0.55"/>

<!-- first order actuator dynamics -->
<define name="ACT_DYN_P" value="0.06"/>
<define name="ACT_DYN_Q" value="0.06"/>
<define name="ACT_DYN_R" value="0.06"/>

<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.0001"/>
</section>

<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="283"/>
<define name="HOVER_KD" value="82"/>
<define name="HOVER_KI" value="13"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.655"/>
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
</section>

<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<!-- Good weather -->
<define name="MAX_BANK" value="20" unit="deg"/>
<!-- Bad weather -->
<!-- define name="MAX_BANK" value="32" unit="deg"/ -->
<define name="PGAIN" value="79"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="30"/>
</section>

<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;nw_motor&quot;, &quot;ne_motor&quot;, &quot;se_motor&quot;, &quot;sw_motor&quot;}"/>
<define name="JSBSIM_MODEL" value="&quot;simple_ardrone2&quot;"/>
<define name="JSBSIM_INIT" value="&quot;reset00&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_ardrone2.h&quot;"/>
</section>

<section name="AUTOPILOT">
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
<define name="MODE_AUTO2" value="AP_MODE_MODULE"/>
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
</section>

<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="8700"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="9.8" unit="V"/>
<define name="LOW_BAT_LEVEL" value="10.2" unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.6" unit="V"/>
</section>
</airframe>
14 changes: 7 additions & 7 deletions conf/modules/opticflow_hover.xml
View file
Expand Up @@ -25,17 +25,17 @@
<dl_settings NAME="Vision stabilization">
<!-- Satabilization loop parameters and gains -->
<dl_settings name="vision_stab">
<dl_setting var="opticflow_stab.phi_pgain" module="computer_vision/opticflow_module" min="0" step="1" max="10000" shortname="kp_v_phi" param="VISION_PHI_PGAIN"/>
<dl_setting var="opticflow_stab.phi_igain" module="computer_vision/opticflow_module" min="0" step="1" max="1000" shortname="ki_v_phi" param="VISION_PHI_IGAIN"/>
<dl_setting var="opticflow_stab.theta_pgain" module="computer_vision/opticflow_module" min="0" step="1" max="10000" shortname="kp_v_theta" param="VISION_THETA_PGAIN"/>
<dl_setting var="opticflow_stab.theta_igain" module="computer_vision/opticflow_module" min="0" step="1" max="1000" shortname="ki_v_theta" param="VISION_THETA_IGAIN"/>
<dl_setting var="opticflow_stab.desired_vx" module="computer_vision/opticflow_module" min="-5" step="0.01" max="5" shortname="desired_vx" param="VISION_DESIRED_VX"/>
<dl_setting var="opticflow_stab.desired_vy" module="computer_vision/opticflow_module" min="-5" step="0.01" max="5" shortname="desired_vy" param="VISION_DESIRED_VY"/>
<dl_setting var="opticflow_stab.phi_pgain" module="guidance_opticflow/guidance_opticflow_hover" min="0" step="1" max="10000" shortname="kp_v_phi" param="VISION_PHI_PGAIN"/>
<dl_setting var="opticflow_stab.phi_igain" module="guidance_opticflow/guidance_opticflow_hover" min="0" step="1" max="1000" shortname="ki_v_phi" param="VISION_PHI_IGAIN"/>
<dl_setting var="opticflow_stab.theta_pgain" module="guidance_opticflow/guidance_opticflow_hover" min="0" step="1" max="10000" shortname="kp_v_theta" param="VISION_THETA_PGAIN"/>
<dl_setting var="opticflow_stab.theta_igain" module="guidance_opticflow/guidance_opticflow_hover" min="0" step="1" max="1000" shortname="ki_v_theta" param="VISION_THETA_IGAIN"/>
<dl_setting var="opticflow_stab.desired_vx" module="guidance_opticflow/guidance_opticflow_hover" min="-5" step="0.01" max="5" shortname="desired_vx" param="VISION_DESIRED_VX"/>
<dl_setting var="opticflow_stab.desired_vy" module="guidance_opticflow/guidance_opticflow_hover" min="-5" step="0.01" max="5" shortname="desired_vy" param="VISION_DESIRED_VY"/>
</dl_settings>
</dl_settings>
</settings>

<depends>cv_opticflow</depends>
<!--depends>cv_opticflow</depends-->

<header>
<file name="guidance_opticflow_hover.h"/>
Expand Down
2 changes: 2 additions & 0 deletions sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.c
View file
Expand Up @@ -151,6 +151,7 @@ void guidance_h_module_run(bool_t in_flight)
static void stabilization_opticflow_vel_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp, float vel_x, float vel_y, float vel_z)
{
printf("Stabilisation before %f %f \n", vel_x, vel_y);
/* Check if we are in the correct AP_MODE before setting commands */
if (autopilot_mode != AP_MODE_MODULE) {
return;
Expand All @@ -170,6 +171,7 @@ static void stabilization_opticflow_vel_cb(uint8_t sender_id __attribute__((unus
opticflow_stab.cmd.theta = -(opticflow_stab.theta_pgain * err_vy / 100
+ opticflow_stab.theta_igain * opticflow_stab.err_vy_int);

// printf("Stabilisation %f %f so we take %f %f\n", vel_x, vel_y, opticflow_stab.cmd.phi, opticflow_stab.cmd.theta);
/* Bound the roll and pitch commands */
BoundAbs(opticflow_stab.cmd.phi, CMD_OF_SAT);
BoundAbs(opticflow_stab.cmd.theta, CMD_OF_SAT);
Expand Down
10 changes: 9 additions & 1 deletion sw/airborne/modules/optical_flow/px4flow.c
View file
Expand Up @@ -28,7 +28,7 @@

#include "modules/optical_flow/px4flow.h"
#include "modules/datalink/mavlink_decoder.h"
#include <string.h>
#include "subsystems/abi.h"

struct mavlink_optical_flow optical_flow;
bool_t optical_flow_available;
Expand All @@ -40,11 +40,19 @@ bool_t optical_flow_available;

// request struct for mavlink decoder
struct mavlink_msg_req req;
#define PIX4FLOWSENDER_ID 1333

// callback function on message reception
static void decode_optical_flow_msg(struct mavlink_message *msg __attribute__((unused)))
{
optical_flow_available = TRUE;
// printf("Stabilisation decoded %d %d height: %f \n", optical_flow.flow_x, optical_flow.flow_y,optical_flow.ground_distance);

// X and Y negated to get to the body of the drone
AbiSendMsgVELOCITY_ESTIMATE(PIX4FLOWSENDER_ID, 0,
(optical_flow.flow_x/optical_flow.ground_distance),
-1.0*(optical_flow.flow_y/optical_flow.ground_distance),
0.0f);
}

/** Initialization function
Expand Down

0 comments on commit a39d163

Please sign in to comment.