Skip to content

Commit

Permalink
Merge pull request #1412 'ins_velocity_estimate'
Browse files Browse the repository at this point in the history 
- ins_int: listen to VELOCITY_ESTIMATE ABI messages and use it as measurement to update horizontal velocity
- add noise field to VELOCITY_ESTIMATE message
- opticflow module: fix velocity scaling and rotate to body frame
- update guidance opticflow hover module accordingly
  • Loading branch information
@flixr
flixr committed Nov 11, 2015
2 parents 0b1a15d + d5dbb5e commit 7c6f23d
Show file tree
Hide file tree
Showing 8 changed files with 319 additions and 20 deletions.
1 change: 1 addition & 0 deletions conf/abi.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,6 +70,7 @@
<field name="x" type="float" unit="m/s"/>
<field name="y" type="float" unit="m/s"/>
<field name="z" type="float" unit="m/s"/>
<field name="noise" type="float"/>
</message>

</msg_class>
Expand Down
231 changes: 231 additions & 0 deletions conf/airframes/TUDelft/airframes/ardrone2_opticflow_ins.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,231 @@
<!DOCTYPE airframe SYSTEM "../../airframe.dtd">

<airframe name="ardrone2_opticflow">

<firmware name="rotorcraft">
<target name="ap" board="ardrone2"/>

<!--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"/>
<subsystem name="gps" type="ublox"/>
<subsystem name="stabilization" type="int_quat"/>
<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="gps_ubx_ucenter.xml"/>
<load name="send_imu_mag_current.xml"/>
<load name="logger_file.xml"/>
<load name="cv_opticflow.xml"/>
<load name="opticflow_hover.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="VISION" prefix="VISION_">
<define name="HOVER" value="FALSE"/>
<define name="PHI_PGAIN" value="1500"/>
<define name="PHI_IGAIN" value="800"/>
<define name="THETA_PGAIN" value="1500"/>
<define name="THETA_IGAIN" value="800"/>
<define name="DESIRED_VX" value="0"/>
<define name="DESIRED_VY" value="0"/>
</section>

<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_">
<!-- Toulouse -->
<define name="H_X" value="0.513081"/>
<define name="H_Y" value="-0.00242783"/>
<define name="H_Z" value="0.858336"/>
<!-- 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="2.2"/>
<define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
</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="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="45" unit="deg"/>
<define name="SP_MAX_THETA" value="45" unit="deg"/>
<define name="SP_MAX_R" value="600" unit="deg/s"/>
<define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="250"/>

<!-- reference -->
<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="200" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.9"/>
<define name="REF_MAX_R" value="300." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(4000.)"/>

<!-- feedback -->
<define name="PHI_PGAIN" value="850"/>
<define name="PHI_DGAIN" value="425"/>
<define name="PHI_IGAIN" value="0"/>

<define name="THETA_PGAIN" value="850"/>
<define name="THETA_DGAIN" value="425"/>
<define name="THETA_IGAIN" value="0"/>

<define name="PSI_PGAIN" value="1000"/>
<define name="PSI_DGAIN" value="700"/>
<define name="PSI_IGAIN" value="0"/>

<!-- feedforward -->
<define name="PHI_DDGAIN" value="0"/>
<define name="THETA_DDGAIN" value="0"/>
<define name="PSI_DDGAIN" value="100"/>
</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"/ -->
<!-- Hover without RC input-->
<define name="GUIDANCE_H_USE_REF" value="FALSE"/>
<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="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_MODULE"/>
<define name="MODE_AUTO2" value="AP_MODE_HOVER_Z_HOLD"/>
<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.6" unit="V"/>
<define name="LOW_BAT_LEVEL" value="9.7" unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
</section>
</airframe>
4 changes: 2 additions & 2 deletions conf/messages.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1972,8 +1972,8 @@
<field name="flow_y" type="int16" unit="subpixels"/>
<field name="flow_der_x" type="int16" unit="subpixels"/>
<field name="flow_der_y" type="int16" unit="subpixels"/>
<field name="vel_x" type="float" unit="cm/s"/>
<field name="vel_y" type="float" unit="cm/s"/>
<field name="vel_x" type="float" unit="m/s"/>
<field name="vel_y" type="float" unit="m/s"/>
<field name="div_size" type="float" unit="1/s"/>
<field name="surface_roughness" type="float" unit="1/s"/>
<field name="divergence" type="float" unit="1/s"/>
Expand Down
2 changes: 2 additions & 0 deletions sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,8 @@ struct opticflow_result_t {

float surface_roughness; ///< Surface roughness as determined with a linear optical flow fit
float divergence; ///< Divergence as determined with a linear flow fit

float noise_measurement; ///< noise of measurement, for state filter
};

/* The state of the drone when it took an image */
Expand Down
31 changes: 26 additions & 5 deletions sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -152,7 +152,8 @@ void opticflow_calc_init(struct opticflow_t *opticflow, uint16_t w, uint16_t h)
* @param[in] *img The image frame to calculate the optical flow from
* @param[out] *result The optical flow result
*/
void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img, struct opticflow_result_t *result)
void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
struct opticflow_result_t *result)
{
// variables for size_divergence:
float size_divergence; int n_samples;
Expand Down Expand Up @@ -230,7 +231,8 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
error_threshold = 10.0f;
n_iterations_RANSAC = 20;
n_samples_RANSAC = 5;
success_fit = analyze_linear_flow_field(vectors, result->tracked_cnt, error_threshold, n_iterations_RANSAC, n_samples_RANSAC, img->w, img->h, &fit_info);
success_fit = analyze_linear_flow_field(vectors, result->tracked_cnt, error_threshold, n_iterations_RANSAC,
n_samples_RANSAC, img->w, img->h, &fit_info);

if (!success_fit) {
fit_info.divergence = 0.0f;
Expand Down Expand Up @@ -276,8 +278,26 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
opticflow->prev_theta = state->theta;

// Velocity calculation
result->vel_x = -result->flow_der_x * result->fps * state->agl / opticflow->subpixel_factor * img->w / OPTICFLOW_FX;
result->vel_y = result->flow_der_y * result->fps * state->agl / opticflow->subpixel_factor * img->h / OPTICFLOW_FY;
// Right now this formula is under assumption that the flow only exist in the center axis of the camera.
// TODO Calculate the velocity more sophisticated, taking into account the drone's angle and the slope of the ground plane.
float vel_hor = result->flow_der_x * result->fps * state->agl / opticflow->subpixel_factor / OPTICFLOW_FX;
float vel_ver = result->flow_der_y * result->fps * state->agl / opticflow->subpixel_factor / OPTICFLOW_FY;

// Velocity calculation: uncomment if focal length of the camera is not known or incorrect.
// result->vel_x = - result->flow_der_x * result->fps * state->agl / opticflow->subpixel_factor * OPTICFLOW_FOV_W / img->w
// result->vel_y = result->flow_der_y * result->fps * state->agl / opticflow->subpixel_factor * OPTICFLOW_FOV_H / img->h

// Rotate velocities from camera frame coordinates to body coordinates.
result->vel_x = vel_ver;
result->vel_y = - vel_hor;

// Determine quality of noise measurement for state filter
//TODO Experiment with multiple noise measurement models
if (result->tracked_cnt < 10) {
result->noise_measurement = (float)result->tracked_cnt / (float)opticflow->max_track_corners;
} else {
result->noise_measurement = 1.0;
}

// *************************************************************************************
// Next Loop Preparation
Expand Down Expand Up @@ -312,7 +332,8 @@ static int cmp_flow(const void *a, const void *b)
{
const struct flow_t *a_p = (const struct flow_t *)a;
const struct flow_t *b_p = (const struct flow_t *)b;
return (a_p->flow_x * a_p->flow_x + a_p->flow_y * a_p->flow_y) - (b_p->flow_x * b_p->flow_x + b_p->flow_y * b_p->flow_y);
return (a_p->flow_x * a_p->flow_x + a_p->flow_y * a_p->flow_y) - (b_p->flow_x * b_p->flow_x + b_p->flow_y *
b_p->flow_y);
}


8 changes: 6 additions & 2 deletions sw/airborne/modules/computer_vision/opticflow_module.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -131,7 +131,8 @@ void opticflow_module_init(void)
#endif

/* Try to initialize the video device */
opticflow_dev = v4l2_init(STRINGIFY(OPTICFLOW_DEVICE), OPTICFLOW_DEVICE_SIZE, OPTICFLOW_DEVICE_BUFFERS, V4L2_PIX_FMT_UYVY);
opticflow_dev = v4l2_init(STRINGIFY(OPTICFLOW_DEVICE), OPTICFLOW_DEVICE_SIZE, OPTICFLOW_DEVICE_BUFFERS,
V4L2_PIX_FMT_UYVY);
if (opticflow_dev == NULL) {
printf("[opticflow_module] Could not initialize the video device\n");
}
Expand Down Expand Up @@ -163,11 +164,14 @@ void opticflow_module_run(void)
opticflow_result.flow_der_x,
quality,
opticflow_state.agl);
//TODO Find an appropiate quality measure for the noise model in the state filter, for now it is tracked_cnt
if (opticflow_result.tracked_cnt > 0) {
AbiSendMsgVELOCITY_ESTIMATE(OPTICFLOW_SENDER_ID, now_ts,
opticflow_result.vel_x,
opticflow_result.vel_y,
0.0f);
0.0f,
opticflow_result.noise_measurement
);
}
opticflow_got_result = FALSE;
}
Expand Down
16 changes: 8 additions & 8 deletions sw/airborne/modules/guidance_opticflow/guidance_opticflow_hover.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,7 +98,7 @@ struct opticflow_stab_t opticflow_stab = {


static void stabilization_opticflow_vel_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp, float vel_x, float vel_y, float vel_z);
uint32_t stamp, float vel_x, float vel_y, float vel_z, float noise);
/**
* Initialization of horizontal guidance module.
*/
Expand Down Expand Up @@ -149,7 +149,7 @@ void guidance_h_module_run(bool_t in_flight)
* Update the controls on a new VELOCITY_ESTIMATE ABI message.
*/
static void stabilization_opticflow_vel_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp, float vel_x, float vel_y, float vel_z)
uint32_t stamp, float vel_x, float vel_y, float vel_z, float noise)
{
/* Check if we are in the correct AP_MODE before setting commands */
if (autopilot_mode != AP_MODE_MODULE) {
Expand All @@ -161,14 +161,14 @@ static void stabilization_opticflow_vel_cb(uint8_t sender_id __attribute__((unus
float err_vy = opticflow_stab.desired_vy - vel_y;

/* Calculate the integrated errors (TODO: bound??) */
opticflow_stab.err_vx_int += err_vx / 100;
opticflow_stab.err_vy_int += err_vy / 100;
opticflow_stab.err_vx_int += err_vx / 512;
opticflow_stab.err_vy_int += err_vy / 512;

/* Calculate the commands */
opticflow_stab.cmd.phi = opticflow_stab.phi_pgain * err_vx / 100
+ opticflow_stab.phi_igain * opticflow_stab.err_vx_int;
opticflow_stab.cmd.theta = -(opticflow_stab.theta_pgain * err_vy / 100
+ opticflow_stab.theta_igain * opticflow_stab.err_vy_int);
opticflow_stab.cmd.phi = opticflow_stab.phi_pgain * err_vy
+ opticflow_stab.phi_igain * opticflow_stab.err_vy_int;
opticflow_stab.cmd.theta = -(opticflow_stab.theta_pgain * err_vx
+ opticflow_stab.theta_igain * opticflow_stab.err_vx_int);

/* Bound the roll and pitch commands */
BoundAbs(opticflow_stab.cmd.phi, CMD_OF_SAT);
Expand Down
Loading

0 comments on commit 7c6f23d

Please sign in to comment.