mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-27 00:37:37 +08:00
Generic rotation matrix for mounted stereocamera (#1998)
This pull request adds a rotation matrix to the stereocam module group. The stereocam in combination with the lisa-s, the new lisa MXs, and the ardrone are usually mounted on there in different direction (facing backwards, forwards and downwards). Before, we had to constantly double check the direction of the velocities to transform that manually to body fixed coordinates, which was very prone to mistakes. Same goes with the derotation of optical flow on the stereocam. This addition should make it easier and more generic for platforms with the mounted stereocamera.
This commit is contained in:
knmcguire
Felix Ruess
@@ -32,4 +32,15 @@
|
||||
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps.xml modules/imu_common.xml"
|
||||
gui_color="blue"
|
||||
/>
|
||||
<aircraft
|
||||
name="LadybirdMXS_wifi_indi_stereocam"
|
||||
ac_id="15"
|
||||
airframe="airframes/TUDELFT/tudelft_ladybird_wifi_indi_lisa_mxs.xml"
|
||||
radio="radios/dummy.xml"
|
||||
telemetry="telemetry/default_rotorcraft.xml"
|
||||
flight_plan="flight_plans/rotorcraft_optitrack.xml"
|
||||
settings="settings/rotorcraft_basic.xml settings/control/stabilization_indi.xml settings/control/rotorcraft_guidance.xml"
|
||||
settings_modules="modules/air_data.xml modules/ahrs_int_cmpl_quat.xml modules/gps.xml modules/imu_common.xml modules/stereocam.xml"
|
||||
gui_color="blue"
|
||||
/>
|
||||
</conf>
|
||||
|
||||
@@ -0,0 +1,300 @@
|
||||
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
|
||||
|
||||
|
||||
<!-- this is a LadyBird quadrotor frame equiped with Lisa/MXS 1.0 -->
|
||||
<!-- The LadyBird frame comes with four brushed motors in an X configuration. -->
|
||||
|
||||
<!--
|
||||
The motor and rotor configuration is the following:
|
||||
|
||||
Front
|
||||
^
|
||||
|
|
||||
|
||||
Motor3(NW) Motor0(NE)
|
||||
CW CCW
|
||||
\ /
|
||||
,___,
|
||||
| |
|
||||
| |
|
||||
|___|
|
||||
/ \
|
||||
CCW CW
|
||||
Motor2(SW) Motor1(SE)
|
||||
|
||||
-->
|
||||
|
||||
<airframe name="quadrotor_lisa_mxs">
|
||||
|
||||
<servos driver="Pwm">
|
||||
<servo name="NE" no="0" min="0" neutral="50" max="1000"/>
|
||||
<servo name="SE" no="5" min="0" neutral="50" max="1000"/>
|
||||
<servo name="SW" no="4" min="0" neutral="50" max="1000"/>
|
||||
<servo name="NW" no="1" min="0" neutral="50" max="1000"/>
|
||||
</servos>
|
||||
|
||||
<commands>
|
||||
<axis name="PITCH" failsafe_value="0"/>
|
||||
<axis name="ROLL" failsafe_value="0"/>
|
||||
<axis name="YAW" failsafe_value="0"/>
|
||||
<axis name="THRUST" failsafe_value="0"/>
|
||||
</commands>
|
||||
|
||||
<command_laws>
|
||||
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
|
||||
<set servo="NE" value="motor_mixing.commands[0]"/>
|
||||
<set servo="SE" value="motor_mixing.commands[1]"/>
|
||||
<set servo="SW" value="motor_mixing.commands[2]"/>
|
||||
<set servo="NW" value="motor_mixing.commands[3]"/>
|
||||
</command_laws>
|
||||
|
||||
<section name="MIXING" prefix="MOTOR_MIXING_">
|
||||
<define name="TRIM_ROLL" value="0"/>
|
||||
<define name="TRIM_PITCH" value="0"/>
|
||||
<define name="TRIM_YAW" value="0"/>
|
||||
<define name="NB_MOTOR" value="4"/>
|
||||
<define name="SCALE" value="256"/>
|
||||
<define name="ROLL_COEF" value="{ -256, -256, 256, 256 }"/>
|
||||
<define name="PITCH_COEF" value="{ 256, -256, -256, 256 }"/>
|
||||
<define name="YAW_COEF" value="{ 256, -256, 256, -256 }"/>
|
||||
<define name="THRUST_COEF" value="{ 256, 256, 256, 256 }"/>
|
||||
</section>
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
<define name="BODY_TO_IMU_PHI" value="-2.0" unit="deg"/>
|
||||
<define name="BODY_TO_IMU_THETA" value="-0.0" unit="deg"/>
|
||||
<define name="BODY_TO_IMU_PSI" value="135." unit="deg"/>
|
||||
|
||||
<define name="ACCEL_X_NEUTRAL" value="0"/>
|
||||
<define name="ACCEL_Y_NEUTRAL" value="0"/>
|
||||
<define name="ACCEL_Z_NEUTRAL" value="0"/>
|
||||
|
||||
<!-- MAGNETO CALIBRATION DELFT -->
|
||||
<define name="MAG_X_NEUTRAL" value="286"/>
|
||||
<define name="MAG_Y_NEUTRAL" value="-72"/>
|
||||
<define name="MAG_Z_NEUTRAL" value="97"/>
|
||||
<define name="MAG_X_SENS" value="3.94431833863" integer="16"/>
|
||||
<define name="MAG_Y_SENS" value="4.14629702271" integer="16"/>
|
||||
<define name="MAG_Z_SENS" value="4.54518768636" integer="16"/>
|
||||
|
||||
<!-- MAGNETO CURRENT CALIBRATION -->
|
||||
<define name="MAG_X_CURRENT_COEF" value="0.0103422023767"/>
|
||||
<define name="MAG_Y_CURRENT_COEF" value="0.0084568317783"/>
|
||||
<define name="MAG_Z_CURRENT_COEF" value="-0.01935617335"/>
|
||||
</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_HOVER_Z_HOLD"/>
|
||||
<define name="MODE_AUTO2" value="AP_MODE_HOVER_DIRECT"/>
|
||||
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
|
||||
</section>
|
||||
|
||||
<section name="BAT">
|
||||
<define name="MILLIAMP_AT_FULL_THROTTLE" value="14000"/>
|
||||
<define name="CATASTROPHIC_BAT_LEVEL" value="3.0" unit="V"/>
|
||||
<define name="CRITIC_BAT_LEVEL" value="3.3" unit="V"/>
|
||||
<define name="LOW_BAT_LEVEL" value="3.5" unit="V"/>
|
||||
<define name="MAX_BAT_LEVEL" value="4.1" unit="V"/>
|
||||
</section>
|
||||
|
||||
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
|
||||
<!-- control effectiveness -->
|
||||
<define name="G1_P" value="0.038174"/>
|
||||
<define name="G1_Q" value="0.063857"/>
|
||||
<define name="G1_R" value="0.002845"/>
|
||||
<define name="G2_R" value="0.128776"/>
|
||||
|
||||
<!-- reference acceleration for attitude control -->
|
||||
<define name="REF_ERR_P" value="170.0"/>
|
||||
<define name="REF_ERR_Q" value="170.0"/>
|
||||
<define name="REF_ERR_R" value="100.0"/>
|
||||
<define name="REF_RATE_P" value="17.0"/>
|
||||
<define name="REF_RATE_Q" value="17.0"/>
|
||||
<define name="REF_RATE_R" value="17.0"/>
|
||||
|
||||
<!-- second order filter parameters -->
|
||||
<define name="FILT_OMEGA" value="50.0"/>
|
||||
<define name="FILT_ZETA" value="0.55"/>
|
||||
<define name="FILT_OMEGA_R" value="50.0"/>
|
||||
<define name="FILT_ZETA_R" value="0.55"/>
|
||||
|
||||
<!-- first order actuator dynamics -->
|
||||
<define name="ACT_DYN_P" value="0.03"/>
|
||||
<define name="ACT_DYN_Q" value="0.03"/>
|
||||
<define name="ACT_DYN_R" value="0.03"/>
|
||||
|
||||
<!-- Adaptive Learning Rate -->
|
||||
<define name="USE_ADAPTIVE" value="FALSE"/>
|
||||
<define name="ADAPTIVE_MU" value="0.0001"/>
|
||||
</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_PSI" value="45." unit="deg"/>
|
||||
<define name="SP_MAX_R" value="90." unit="deg/s"/>
|
||||
<define name="SP_MAX_P" value="90." unit="deg/s"/>
|
||||
<define name="DEADBAND_R" value="250"/>
|
||||
<define name="DEADBAND_A" value="250"/>
|
||||
|
||||
<!-- reference -->
|
||||
<define name="REF_OMEGA_P" value="800" unit="deg/s"/>
|
||||
<define name="REF_ZETA_P" value="0.85"/>
|
||||
<define name="REF_MAX_P" value="400." unit="deg/s"/>
|
||||
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
|
||||
|
||||
<define name="REF_OMEGA_Q" value="800" unit="deg/s"/>
|
||||
<define name="REF_ZETA_Q" value="0.85"/>
|
||||
<define name="REF_MAX_Q" value="400." unit="deg/s"/>
|
||||
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
|
||||
|
||||
<define name="REF_OMEGA_R" value="500" unit="deg/s"/>
|
||||
<define name="REF_ZETA_R" value="0.85"/>
|
||||
<define name="REF_MAX_R" value="90." unit="deg/s"/>
|
||||
<define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>
|
||||
|
||||
<!-- feedback -->
|
||||
<define name="PHI_PGAIN" value="400"/>
|
||||
<define name="PHI_DGAIN" value="200"/>
|
||||
<define name="PHI_IGAIN" value="100"/>
|
||||
|
||||
<define name="THETA_PGAIN" value="400"/>
|
||||
<define name="THETA_DGAIN" value="200"/>
|
||||
<define name="THETA_IGAIN" value="100"/>
|
||||
|
||||
<define name="PSI_PGAIN" value="500"/>
|
||||
<define name="PSI_DGAIN" value="300"/>
|
||||
<define name="PSI_IGAIN" value="10"/>
|
||||
|
||||
<!-- feedforward -->
|
||||
<define name="PHI_DDGAIN" value="0"/>
|
||||
<define name="THETA_DDGAIN" value="0"/>
|
||||
<define name="PSI_DDGAIN" value=" 300"/>
|
||||
|
||||
<define name="PHI_AGAIN" value="0"/>
|
||||
<define name="THETA_AGAIN" value="0"/>
|
||||
<define name="PSI_AGAIN" value="0"/>
|
||||
</section>
|
||||
|
||||
<section name="INS" prefix="INS_">
|
||||
<!--define name="INT_GPS_ID" value="ABI_DISABLE"/-->
|
||||
<define name="USE_GPS_ALT" value="true"/>
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
|
||||
<define name="HOVER_KP" value="331"/>
|
||||
<define name="HOVER_KD" value="546"/>
|
||||
<define name="HOVER_KI" value="120"/>
|
||||
<define name="NOMINAL_HOVER_THROTTLE" value="0.65"/>
|
||||
<define name="ADAPT_THROTTLE_ENABLED" value="true"/>
|
||||
</section>
|
||||
|
||||
<section name="AHRS" prefix="AHRS_">
|
||||
<define name="H_X" value=" 0.47577"/>
|
||||
<define name="H_Y" value=" 0.11811"/>
|
||||
<define name="H_Z" value=" 0.87161"/>
|
||||
|
||||
<define name="GRAVITY_HEURISTIC_FACTOR" value="0"/>
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
|
||||
<define name="PGAIN" value="80"/>
|
||||
<define name="DGAIN" value="260"/>
|
||||
<define name="IGAIN" value="0"/>
|
||||
<define name="MAX_BANK" value="RadOfDeg(15)"/>
|
||||
<define name="REF_MAX_SPEED" value="0.5"/>
|
||||
<define name="USE_SPEED_REF" value="TRUE"/>
|
||||
|
||||
</section>
|
||||
|
||||
<section name="MISC">
|
||||
<define name="VoltageOfAdc(adc)" value="(adc)*0.00162f" />
|
||||
</section>
|
||||
|
||||
<section name="SIMULATOR" prefix="NPS_">
|
||||
<define name="ACTUATOR_NAMES" value="ne_motor, se_motor, sw_motor, nw_motor" type="string[]"/>
|
||||
<define name="JSBSIM_MODEL" value="simple_x_quad_ccw" type="string"/>
|
||||
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
|
||||
</section>
|
||||
|
||||
<modules main_freq="512">
|
||||
<!--module name="gps" type="ubx_ucenter"/-->
|
||||
<module name="send_imu_mag_current"/>
|
||||
<module name="stereocam">
|
||||
<define name= "STEREO_BODY_TO_STEREO_PHI" value="90" unit="deg"/>
|
||||
<define name= "STEREO_BODY_TO_STEREO_THETA" value="0" unit="deg"/>
|
||||
<define name= "STEREO_BODY_TO_STEREO_PSI" value="-90" unit="deg"/>
|
||||
<configure name="STEREO_UART" value="UART2"/>
|
||||
<configure name="STEREO_BAUD" value="B115200"/>
|
||||
<define name="SEND_STEREO" value="FALSE"/>
|
||||
</module>
|
||||
<module name="stereocam_state2camera">
|
||||
<configure name="STEREO_UART" value="UART2"/>
|
||||
<configure name="STEREO_BAUD" value="B115200"/>
|
||||
<define name="STATE2CAMERA_SEND_DATA_TYPE" value="1"/>
|
||||
</module>
|
||||
<module name="stereocam_stereocam2state">
|
||||
<define name="STEREOCAM2STATE_RECEIVED_DATA_TYPE" value="0"/>
|
||||
<define name="STEREOCAM2STATE_EDGEFLOW_PIXELWISE" value="TRUE"/>
|
||||
</module>
|
||||
</modules>
|
||||
|
||||
<firmware name="rotorcraft">
|
||||
<target name="ap" board="lisa_mx_2.1">
|
||||
|
||||
<define name="REMAP_UART3" value="TRUE" />
|
||||
|
||||
<module name="radio_control" type="datalink"/>
|
||||
<!--module name="radio_control" type="spektrum">
|
||||
<configure name="RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT" value="UART5" />
|
||||
</module-->
|
||||
|
||||
<configure name="AHRS_PROPAGATE_FREQUENCY" value="500"/>
|
||||
<define name="USE_PERSISTENT_SETTINGS" value="TRUE"/>
|
||||
</target>
|
||||
|
||||
<target name="nps" board="pc">
|
||||
<module name="fdm" type="jsbsim"/>
|
||||
<module name="radio_control" type="ppm"/>
|
||||
</target>
|
||||
|
||||
<module name="motor_mixing"/>
|
||||
<module name="actuators" type="pwm">
|
||||
<!-- This airframe is using the servo PWM outputs directly to drive mosfets, which in turn directly drive brushed motors.
|
||||
With the following settings we will be driving the motors with a 36KHz signal that should be well above anyone's hearing range.
|
||||
If we went with the 500Hz that woul be the default we would be hearing a very annoying high pitched sound coming from the motors.
|
||||
-->
|
||||
<!-- Setting the PWM timer base frequency to 36MHz -->
|
||||
<define name="PWM_BASE_FREQ" value="36000000"/>
|
||||
<!-- Setting the PWM interval to 36KHz -->
|
||||
<define name="SERVO_HZ" value="36000"/>
|
||||
</module>
|
||||
|
||||
<module name="telemetry" type="transparent">
|
||||
<configure name="MODEM_PORT" value="UART1"/>
|
||||
<configure name="MODEM_BAUD" value="B115200"/>
|
||||
</module>
|
||||
|
||||
<module name="imu" type="lisa_mx_v2.1"/>
|
||||
|
||||
<module name="gps" type="optitrack">
|
||||
<!--configure name="GPS_PORT" value="UART2"/>
|
||||
<configure name="GPS_BAUD" value="B38400"/-->
|
||||
</module>
|
||||
|
||||
<!--module name="stabilization" type="int_quat"/-->
|
||||
<module name="stabilization" type="indi_simple"/>
|
||||
<module name="stabilization" type="rate_indi"/>
|
||||
<module name="ahrs" type="int_cmpl_quat"/>
|
||||
<module name="ins"/>
|
||||
<configure name="USE_MAGNETOMETER" value="FALSE"/>
|
||||
<module name="air_data">
|
||||
<define name="AIR_DATA_CALC_AMSL_BARO" value="TRUE"/>
|
||||
</module>
|
||||
</firmware>
|
||||
</airframe>
|
||||
@@ -14,6 +14,18 @@
|
||||
<define name="SEND_STEREO" value="TRUE|FALSE" description="If TRUE, a copy of received messages is sent to the ground station over the datalink (default: TRUE)"/>
|
||||
<define name="STEREO_BUF_SIZE" value="NUM" description="Size of the receiving buffer, must be at least as large as the message being sent (default: 1024)"/>
|
||||
</doc>
|
||||
<settings>
|
||||
<dl_settings>
|
||||
<dl_settings name="stereocam">
|
||||
<dl_setting var="stereocam_derotation_on" min="0" step="1" max="1" shortname="derotation"
|
||||
module="stereocam/state2camera/state2camera">
|
||||
</dl_setting>
|
||||
<dl_setting var="stereocam_medianfilter_on" min="0" step="1" max="1" shortname="median_filter"
|
||||
module="stereocam/stereocam2state/stereocam2state">
|
||||
</dl_setting>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
<header>
|
||||
<file name="stereocam.h"/>
|
||||
</header>
|
||||
|
||||
@@ -13,7 +13,7 @@
|
||||
<header>
|
||||
<file name="state2camera/state2camera.h"/>
|
||||
</header>
|
||||
<periodic fun="write_serial_rot()" freq="512."/>
|
||||
<periodic fun="write_serial_rot()" freq="30."/>
|
||||
<makefile>
|
||||
<file name="state2camera/state2camera.c"/>
|
||||
<file name="stereoprotocol.c" dir="modules/stereocam"/>
|
||||
|
||||
@@ -24,20 +24,30 @@
|
||||
*/
|
||||
|
||||
#include "modules/stereocam/state2camera/state2camera.h"
|
||||
#include "modules/stereocam/stereocam.h"
|
||||
#include "modules/stereocam/stereoprotocol.h"
|
||||
#include "subsystems/abi.h"
|
||||
#include "state.h"
|
||||
#include "mcu_periph/uart.h"
|
||||
static int frame_number_sending = 0;
|
||||
float lastKnownHeight = 0.0;
|
||||
int pleaseResetOdroid = 0;
|
||||
|
||||
#ifndef STATE2CAMERA_SEND_DATA_TYPE
|
||||
#define STATE2CAMERA_SEND_DATA_TYPE 0
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(STATE2CAMERA_SEND_DATA_TYPE)
|
||||
|
||||
#if STATE2CAMERA_SEND_DATA_TYPE == 0
|
||||
static int frame_number_sending = 0;
|
||||
#endif
|
||||
#if STATE2CAMERA_SEND_DATA_TYPE == 1
|
||||
uint8_t stereocam_derotation_on = 1;
|
||||
#endif
|
||||
|
||||
|
||||
void write_serial_rot()
|
||||
{
|
||||
// 0 = send rotation matrix to stereocam
|
||||
#if STATE2CAMERA_SEND_DATA_TYPE == 0
|
||||
|
||||
struct Int32RMat *ltp_to_body_mat = stateGetNedToBodyRMat_i();
|
||||
@@ -52,15 +62,31 @@ void write_serial_rot()
|
||||
stereoprot_sendArray(&((UART_LINK).device), ar, lengthArrayInformation, 1);
|
||||
#endif
|
||||
|
||||
// 0 = send euler angles to stereocam (EdgeFlow)
|
||||
#if STATE2CAMERA_SEND_DATA_TYPE == 1
|
||||
|
||||
|
||||
// rotate body angles to camera reference frame
|
||||
// TODO: When available in paparazzi for matrix multiplications, use FloatEulers
|
||||
|
||||
struct FloatVect3 body_state;
|
||||
body_state.x = stateGetNedToBodyEulers_f()->phi;
|
||||
body_state.y = stateGetNedToBodyEulers_f()->theta;
|
||||
body_state.z = stateGetNedToBodyEulers_f()->psi;
|
||||
|
||||
struct FloatVect3 cam_angles;
|
||||
float_rmat_vmult(&cam_angles, &body_to_stereocam, &body_state);
|
||||
|
||||
static int16_t lengthArrayInformation = 6 * sizeof(int16_t);
|
||||
uint8_t ar[lengthArrayInformation];
|
||||
int16_t *pointer = (int16_t *) ar;
|
||||
pointer[0] = (int16_t)(stateGetNedToBodyEulers_f()->theta*100);
|
||||
pointer[1] = (int16_t)(stateGetNedToBodyEulers_f()->phi*100);
|
||||
pointer[2] = (int16_t)(stateGetNedToBodyEulers_f()->psi*100);
|
||||
pointer[0] = (int16_t)(cam_angles.x * 100); // Roll
|
||||
pointer[1] = (int16_t)(cam_angles.y * 100); // Pitch
|
||||
pointer[2] = (int16_t)(cam_angles.z * 100); // Yaw
|
||||
pointer[3] = (int16_t)(stereocam_derotation_on); // derotation boolean
|
||||
|
||||
stereoprot_sendArray(&((UART_LINK).device), ar, lengthArrayInformation, 1);
|
||||
|
||||
stereoprot_sendArray(&((UART_LINK).device), ar,lengthArrayInformation, 1);
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
@@ -28,6 +28,6 @@
|
||||
|
||||
#include <inttypes.h>
|
||||
extern void write_serial_rot(void);
|
||||
|
||||
extern uint8_t stereocam_derotation_on;
|
||||
#endif
|
||||
|
||||
|
||||
@@ -35,6 +35,42 @@
|
||||
#define SEND_STEREO TRUE
|
||||
#endif
|
||||
|
||||
|
||||
/* This defines the location of the stereocamera with respect to the body fixed coordinates.
|
||||
*
|
||||
* Coordinate system stereocam (looking into the lens)
|
||||
* x z
|
||||
* <-----(( (*) )) (( )) = camera lens
|
||||
* | (*) = arrow pointed outwards
|
||||
* | y (towards your direction)
|
||||
* V
|
||||
*
|
||||
* The conversion order in euler angles is psi (yaw) -> theta (pitch) -> phi (roll)
|
||||
*
|
||||
* Standard rotations: MAV NED body to stereocam in Deg:
|
||||
* - facing forward: 90 -> 0 -> 90
|
||||
* - facing backward: -90 -> 0 -> 90
|
||||
* - facing downward: 90 -> 0 -> 0
|
||||
*/
|
||||
|
||||
#if !defined(STEREO_BODY_TO_STEREO_PHI) || !defined(STEREO_BODY_TO_STEREO_THETA) || !defined(STEREO_BODY_TO_STEREO_PSI)
|
||||
#warning "STEREO_BODY_TO_STEREO_XXX not defined. Using default Euler rotation angles (0,0,0)"
|
||||
#endif
|
||||
|
||||
#ifndef STEREO_BODY_TO_STEREO_PHI
|
||||
#define STEREO_BODY_TO_STEREO_PHI 0
|
||||
#endif
|
||||
|
||||
#ifndef STEREO_BODY_TO_STEREO_THETA
|
||||
#define STEREO_BODY_TO_STEREO_THETA 0
|
||||
#endif
|
||||
|
||||
#ifndef STEREO_BODY_TO_STEREO_PSI
|
||||
#define STEREO_BODY_TO_STEREO_PSI 0
|
||||
#endif
|
||||
|
||||
struct FloatRMat body_to_stereocam;
|
||||
|
||||
// define coms link for stereocam
|
||||
#define STEREO_PORT (&((UART_LINK).device))
|
||||
struct link_device *linkdev = STEREO_PORT;
|
||||
@@ -43,7 +79,6 @@ struct link_device *linkdev = STEREO_PORT;
|
||||
// pervasive local variables
|
||||
MsgProperties msgProperties;
|
||||
|
||||
|
||||
uint16_t freq_counter = 0;
|
||||
uint8_t frequency = 0;
|
||||
uint32_t previous_time = 0;
|
||||
@@ -76,6 +111,9 @@ extern void stereocam_disparity_to_meters(uint8_t *disparity, float *distancesMe
|
||||
|
||||
extern void stereocam_start(void)
|
||||
{
|
||||
struct FloatEulers euler = {STEREO_BODY_TO_STEREO_PHI, STEREO_BODY_TO_STEREO_THETA, STEREO_BODY_TO_STEREO_PSI};
|
||||
float_rmat_of_eulers(&body_to_stereocam, &euler);
|
||||
|
||||
// initialize local variables
|
||||
msgProperties = (MsgProperties) {0, 0, 0};
|
||||
|
||||
@@ -110,11 +148,9 @@ extern void stereocam_periodic(void)
|
||||
#if SEND_STEREO
|
||||
if (stereocam_data.len > 100) {
|
||||
DOWNLINK_SEND_STEREO_IMG(DefaultChannel, DefaultDevice, &frequency, &(stereocam_data.len), 100, stereocam_data.data);
|
||||
|
||||
} else {
|
||||
DOWNLINK_SEND_STEREO_IMG(DefaultChannel, DefaultDevice, &frequency, &(stereocam_data.len), stereocam_data.len,
|
||||
stereocam_data.data);
|
||||
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
@@ -38,6 +38,7 @@ typedef struct {
|
||||
} uint8array;
|
||||
|
||||
extern uint8array stereocam_data;
|
||||
extern struct FloatRMat body_to_stereocam;
|
||||
|
||||
extern void stereocam_disparity_to_meters(uint8_t *, float *, int);
|
||||
extern void stereocam_start(void);
|
||||
|
||||
@@ -22,6 +22,18 @@
|
||||
#ifndef STEREOCAM2STATE_RECEIVED_DATA_TYPE
|
||||
#define STEREOCAM2STATE_RECEIVED_DATA_TYPE 0
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(STEREOCAM2STATE_RECEIVED_DATA_TYPE)
|
||||
|
||||
#if STEREOCAM2STATE_RECEIVED_DATA_TYPE == 0
|
||||
#ifndef STEREOCAM2STATE_EDGEFLOW_PIXELWISE
|
||||
#define STEREOCAM2STATE_EDGEFLOW_PIXELWISE FALSE
|
||||
PRINT_CONFIG_VAR(STEREOCAM2STATE_EDGEFLOW_PIXELWISE)
|
||||
#endif
|
||||
uint8_t stereocam_medianfilter_on = 1;
|
||||
#endif
|
||||
|
||||
#include "filters/median_filter.h"
|
||||
struct MedianFilterInt medianfilter_x, medianfilter_y, medianfilter_z;
|
||||
|
||||
#include "subsystems/datalink/telemetry.h"
|
||||
|
||||
@@ -30,23 +42,28 @@ void stereocam_to_state(void);
|
||||
void stereo_to_state_init(void)
|
||||
{
|
||||
|
||||
init_median_filter(&medianfilter_x);
|
||||
init_median_filter(&medianfilter_y);
|
||||
init_median_filter(&medianfilter_z);
|
||||
}
|
||||
|
||||
void stereo_to_state_periodic(void)
|
||||
{
|
||||
if (stereocam_data.fresh) {
|
||||
stereocam_to_state();
|
||||
stereocam_to_state();
|
||||
stereocam_data.fresh = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void stereocam_to_state(void)
|
||||
{
|
||||
int16_t RES = 100;
|
||||
|
||||
// Sort the info from stereocam data from UART
|
||||
|
||||
// Get info from stereocam data
|
||||
// 0 = stereoboard's #define SEND_EDGEFLOW
|
||||
#if STEREOCAM2STATE_RECEIVED_DATA_TYPE == 0
|
||||
// opticflow
|
||||
// opticflow and divergence (unscaled with depth)
|
||||
int16_t div_x = (int16_t)stereocam_data.data[0] << 8;
|
||||
div_x |= (int16_t)stereocam_data.data[1];
|
||||
int16_t flow_x = (int16_t)stereocam_data.data[2] << 8;
|
||||
@@ -56,48 +73,74 @@ void stereocam_to_state(void)
|
||||
int16_t flow_y = (int16_t)stereocam_data.data[6] << 8;
|
||||
flow_y |= (int16_t)stereocam_data.data[7];
|
||||
|
||||
// uint8_t agl = stereocam_data.data[8]; // in cm //TODO: use agl for in a guided obstacle avoidance.
|
||||
float fps = (float)stereocam_data.data[9];
|
||||
//int8_t agl = stereocam_data.data[8]; // in cm
|
||||
|
||||
// velocity
|
||||
int16_t vel_x_int = (int16_t)stereocam_data.data[10] << 8;
|
||||
vel_x_int |= (int16_t)stereocam_data.data[11];
|
||||
int16_t vel_y_int = (int16_t)stereocam_data.data[12] << 8;
|
||||
vel_y_int |= (int16_t)stereocam_data.data[13];
|
||||
// velocity global
|
||||
int16_t vel_x_global_int = (int16_t)stereocam_data.data[10] << 8;
|
||||
vel_x_global_int |= (int16_t)stereocam_data.data[11];
|
||||
int16_t vel_y_global_int = (int16_t)stereocam_data.data[12] << 8;
|
||||
vel_y_global_int |= (int16_t)stereocam_data.data[13];
|
||||
int16_t vel_z_global_int = (int16_t)stereocam_data.data[14] << 8;
|
||||
vel_z_global_int |= (int16_t)stereocam_data.data[15];
|
||||
|
||||
int16_t RES = 100;
|
||||
// Velocity Pixelwise
|
||||
int16_t vel_x_pixelwise_int = (int16_t)stereocam_data.data[16] << 8;
|
||||
vel_x_pixelwise_int |= (int16_t)stereocam_data.data[17];
|
||||
int16_t vel_z_pixelwise_int = (int16_t)stereocam_data.data[18] << 8;
|
||||
vel_z_pixelwise_int |= (int16_t)stereocam_data.data[19];
|
||||
|
||||
float vel_x = (float)vel_x_int / RES;
|
||||
float vel_y = (float)vel_y_int / RES;
|
||||
// Select what type of velocity estimate fom edgeflow is wanted
|
||||
#if STEREOCAM2STATE_EDGEFLOW_PIXELWISE == TRUE
|
||||
struct FloatVect3 camera_frame_vel;
|
||||
camera_frame_vel.x = (float)vel_x_pixelwise_int / RES;
|
||||
camera_frame_vel.y = (float)vel_y_global_int / RES;
|
||||
camera_frame_vel.z = (float)vel_z_pixelwise_int / RES;
|
||||
|
||||
// Derotate velocity and transform from frame to body coordinates
|
||||
// TODO: send resolution directly from stereocam
|
||||
#else
|
||||
struct FloatVect3 camera_frame_vel;
|
||||
camera_frame_vel.x = (float)vel_x_global_int / RES;
|
||||
camera_frame_vel.y = (float)vel_y_global_int / RES;
|
||||
camera_frame_vel.z = (float)vel_z_global_int / RES;
|
||||
|
||||
float vel_body_x = - vel_x;
|
||||
float vel_body_y = vel_y;
|
||||
#endif
|
||||
|
||||
//Rotate velocity back to quad's frame
|
||||
struct FloatVect3 quad_body_vel;
|
||||
float_rmat_transp_vmult(&quad_body_vel, &body_to_stereocam, &camera_frame_vel);
|
||||
|
||||
//Send velocity estimate to state
|
||||
//TODO:: Make variance dependable on line fit error, after new horizontal filter is made
|
||||
uint32_t now_ts = get_sys_time_usec();
|
||||
|
||||
if (!(abs(vel_body_x) > 0.5 || abs(vel_body_x) > 0.5))
|
||||
{
|
||||
AbiSendMsgVELOCITY_ESTIMATE(STEREOCAM2STATE_SENDER_ID, now_ts,
|
||||
vel_body_x,
|
||||
vel_body_y,
|
||||
0.0f,
|
||||
0.3f
|
||||
);
|
||||
float vel_body_x_processed = quad_body_vel.x;
|
||||
float vel_body_y_processed = quad_body_vel.y;
|
||||
float vel_body_z_processed = quad_body_vel.z;
|
||||
|
||||
if (stereocam_medianfilter_on == 1) {
|
||||
// Use a slight median filter to filter out the large outliers before sending it to state
|
||||
// TODO: if a float median filter exist, replace this version
|
||||
vel_body_x_processed = (float)update_median_filter(&medianfilter_x, (int32_t)(quad_body_vel.x * 100)) / 100;
|
||||
vel_body_y_processed = (float)update_median_filter(&medianfilter_y, (int32_t)(quad_body_vel.y * 100)) / 100;
|
||||
vel_body_z_processed = (float)update_median_filter(&medianfilter_z, (int32_t)(quad_body_vel.z * 100)) / 100;
|
||||
}
|
||||
|
||||
// Reusing the OPTIC_FLOW_EST telemetry messages, with some values replaced by 0
|
||||
//Send velocities to state
|
||||
AbiSendMsgVELOCITY_ESTIMATE(STEREOCAM2STATE_SENDER_ID, now_ts,
|
||||
vel_body_x_processed,
|
||||
vel_body_y_processed,
|
||||
vel_body_z_processed,
|
||||
0.3f
|
||||
);
|
||||
|
||||
// Reusing the OPTIC_FLOW_EST telemetry messages, with some values replaced by 0
|
||||
uint16_t dummy_uint16 = 0;
|
||||
int16_t dummy_int16 = 0;
|
||||
float dummy_float = 0;
|
||||
|
||||
DOWNLINK_SEND_OPTIC_FLOW_EST(DefaultChannel, DefaultDevice, &fps, &dummy_uint16, &dummy_uint16, &flow_x, &flow_y, &dummy_int16, &dummy_int16,
|
||||
&vel_x, &vel_y,&dummy_float, &dummy_float, &dummy_float);
|
||||
DOWNLINK_SEND_OPTIC_FLOW_EST(DefaultChannel, DefaultDevice, &fps, &dummy_uint16, &dummy_uint16, &flow_x, &flow_y,
|
||||
&dummy_int16, &dummy_int16, &vel_body_x_processed, &vel_body_y_processed,
|
||||
&dummy_float, &dummy_float, &dummy_float);
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
@@ -16,6 +16,8 @@
|
||||
#include <std.h>
|
||||
#include "modules/stereocam/stereocam.h"
|
||||
|
||||
extern uint8_t stereocam_medianfilter_on;
|
||||
|
||||
extern void stereo_to_state_init(void);
|
||||
extern void stereo_to_state_periodic(void);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user