Skip to content

Commit

Permalink
Solved rotorcraft problems
Browse files Browse the repository at this point in the history
  • Loading branch information
@xgibert
xgibert committed Jun 7, 2012
1 parent 768aefd commit aa4faaa
Show file tree
Hide file tree
Showing 4 changed files with 49 additions and 30 deletions.
50 changes: 29 additions & 21 deletions conf/messages/downlink.xml
View file
Expand Up @@ -117,7 +117,7 @@
<field name="yaw" type="int16" />
</message>

<message name="BOOZ2_CMD" id="27">
<message name="ROTORCRAFT_CMD" id="27">
<field name="cmd_roll" type="int32"/>
<field name="cmd_pitch" type="int32"/>
<field name="cmd_yaw" type="int32"/>
Expand All @@ -139,7 +139,7 @@
<field name="pitch" type="int8"/>
</message>

<message name="BOOZ2_RADIO_CONTROL" id="31">
<message name="ROTORCRAFT_RADIO_CONTROL" id="31">
<field name="roll" type="int16" unit="pprz"/>
<field name="pitch" type="int16" unit="pprz"/>
<field name="yaw" type="int16" unit="pprz"/>
Expand Down Expand Up @@ -394,15 +394,22 @@
<field name="psi" type="float" unit="rad" alt_unit="deg"/>
</message>

<!-- Missing AHRS_EULER_INT -->
<message name="AHRS_EULER_INT" id="66">
<field name="imu_phi" type="int32" alt_unit="deg" alt_unit_coef="0.0139882"/>
<field name="imu_theta" type="int32" alt_unit="deg" alt_unit_coef="0.0139882"/>
<field name="imu_psi" type="int32" alt_unit="deg" alt_unit_coef="0.0139882"/>
<field name="body_phi" type="int32" alt_unit="deg" alt_unit_coef="0.0139882"/>
<field name="body_theta" type="int32" alt_unit="deg" alt_unit_coef="0.0139882"/>
<field name="body_psi" type="int32" alt_unit="deg" alt_unit_coef="0.0139882"/>
</message>

<message name="AHRS_MEASUREMENT_EULER" id="66">
<message name="AHRS_MEASUREMENT_EULER" id="67">
<field name="phi" type="float" unit="rad" alt_unit="deg"/>
<field name="theta" type="float" unit="rad" alt_unit="deg"/>
<field name="psi" type="float" unit="rad" alt_unit="deg"/>
</message>

<message name="AHRS_DEBUG_QUAT" id="67">
<message name="AHRS_DEBUG_QUAT" id="68">
<field name="jqi" type="float"/>
<field name="jqx" type="float"/>
<field name="jqy" type="float"/>
Expand All @@ -417,7 +424,7 @@
<field name="mqz" type="float"/>
</message>

<message name="AHRS_LKF" id="68">
<message name="AHRS_LKF" id="69">
<field name="phi" type="float" unit="rad" alt_unit="deg"/>
<field name="theta" type="float" unit="rad" alt_unit="deg"/>
<field name="psi" type="float" unit="rad" alt_unit="deg"/>
Expand All @@ -436,7 +443,7 @@
<field name="mz" type="float" />
</message>

<message name="AHRS_LKF_DEBUG" id="69">
<message name="AHRS_LKF_DEBUG" id="70">
<field name="phi_err" type="float" unit="rad" alt_unit="deg"/>
<field name="theta_err" type="float" unit="rad" alt_unit="deg"/>
<field name="psi_err" type="float" unit="rad" alt_unit="deg"/>
Expand All @@ -454,7 +461,7 @@
<field name="br_cov" type="float" />
</message>

<message name="AHRS_LKF_ACC_DBG" id="70">
<message name="AHRS_LKF_ACC_DBG" id="71">
<field name="qi_err" type="float" />
<field name="qx_err" type="float" />
<field name="qy_err" type="float" />
Expand All @@ -464,7 +471,7 @@
<field name="br_err" type="float" unit="rad/s" alt_unit="deg/s"/>
</message>

<message name="AHRS_LKF_MAG_DBG" id="71">
<message name="AHRS_LKF_MAG_DBG" id="72">
<field name="qi_err" type="float" />
<field name="qx_err" type="float" />
<field name="qy_err" type="float" />
Expand All @@ -475,7 +482,7 @@
</message>


<message name="BOOZ2_AHRS_EULER" id="72">
<message name="BOOZ2_AHRS_EULER" id="73">
<field name="imu_phi" type="int32" alt_unit="degres" alt_unit_coef="0.0139882"/>
<field name="imu_theta" type="int32" alt_unit="degres" alt_unit_coef="0.0139882"/>
<field name="imu_psi" type="int32" alt_unit="degres" alt_unit_coef="0.0139882"/>
Expand All @@ -484,7 +491,7 @@
<field name="body_psi" type="int32" alt_unit="degres" alt_unit_coef="0.0139882"/>
</message>

<message name="BOOZ2_AHRS_QUAT" id="73">
<message name="BOOZ2_AHRS_QUAT" id="74">
<field name="imu_qi" type="int32" alt_unit="" alt_unit_coef="0.0000305"/>
<field name="imu_qx" type="int32" alt_unit="" alt_unit_coef="0.0000305"/>
<field name="imu_qy" type="int32" alt_unit="" alt_unit_coef="0.0000305"/>
Expand All @@ -495,7 +502,7 @@
<field name="body_qz" type="int32" alt_unit="" alt_unit_coef="0.0000305"/>
</message>

<message name="BOOZ2_AHRS_RMAT" id="74">
<message name="BOOZ2_AHRS_RMAT" id="75">
<field name="imu_m00" type="int32" alt_unit="" alt_unit_coef="0.0000610"/>
<field name="imu_m01" type="int32" alt_unit="" alt_unit_coef="0.0000610"/>
<field name="imu_m02" type="int32" alt_unit="" alt_unit_coef="0.0000610"/>
Expand All @@ -517,7 +524,7 @@
</message>


<message name="BOOZ_AHRS_BIAS" id="75">
<message name="BOOZ_AHRS_BIAS" id="76">
<field name="bp" type="int32" alt_unit="deg/s" alt_unit_coef="0.0139882"/>
<field name="bq" type="int32" alt_unit="deg/s" alt_unit_coef="0.0139882"/>
<field name="br" type="int32" alt_unit="deg/s" alt_unit_coef="0.0139882"/>
Expand Down Expand Up @@ -1022,14 +1029,15 @@
</message>

<message name="FILTER_ALIGNER" id="162">
<field name="lp_gp" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
<field name="lp_gq" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
<field name="lp_gr" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
<field name="gp" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
<field name="gq" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
<field name="gr" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
<field name="noise" type="int32"/>
<field name="cnt" type="int32"/>
<field name="lp_gp" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
<field name="lp_gq" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
<field name="lp_gr" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
<field name="gp" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
<field name="gq" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
<field name="gr" type="int32" alt_unit="degres/s" alt_unit_coef="0.0139882"/>
<field name="noise" type="int32"/>
<field name="cnt" type="int32"/>
<field name="status" type="uint8" values="UNINIT|RUNNING|LOCKED"/>
</message>

<message name="FILTER_Q" id="163">
Expand Down
10 changes: 5 additions & 5 deletions conf/telemetry/default_rotorcraft.xml
View file
Expand Up @@ -12,7 +12,7 @@
<message name="ROTORCRAFT_FP" period="0.25"/>
<message name="ALIVE" period="2.1"/>
<message name="INS_REF" period="5.1"/>
<message name="MISION_STATUS" period="1.6"/>
<message name="MISSION_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/>
<message name="BOOZ2_CAM" period="1."/>
<message name="GPS_INT" period=".25"/>
Expand Down Expand Up @@ -42,9 +42,9 @@
<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"/>
<message name="IMU_GYRO_INT" period=".075"/>
<message name="IMU_ACCEL_INT" period=".075"/>
<message name="IMU_MAG_INT" period=".1"/>
</mode>

<mode name="ahrs">
Expand Down Expand Up @@ -99,7 +99,7 @@
<!--<message name="STAB_ATTITUDE_REF" period=".4"/>-->
<message name="ROTORCRAFT_FP" period="0.8"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="MISION_STATUS" period="1.6"/>
<message name="MISSION_STATUS" period="1.6"/>
<message name="HFF_GPS" period=".03"/>
<message name="INS_REF" period="5.1"/>
</mode>
Expand Down
4 changes: 3 additions & 1 deletion sw/airborne/firmwares/fixedwing/ap_downlink.h
View file
Expand Up @@ -81,7 +81,9 @@
DOWNLINK_SEND_MISSION_STATUS(_trans, _dev, &estimator_flight_time, &nav_block, &block_time, &nav_stage, &stage_time, &nb_sec, &gps.fix, &dist2_to_wp, &dist2_to_home, &_circle_count, &nav_oval_count, &horizontal_mode);\
})

#define PERIODIC_SEND_MISSION_STATUS(_trans, _dev) SEND_MISSION_STATUS(_trans, _dev)
#define PERIODIC_SEND_MISSION_STATUS(_trans, _dev) Downlink({ \
SEND_MISSION_STATUS(_trans, _dev); \
})

#ifdef MCU_SPI_LINK
#define PERIODIC_SEND_DEBUG_MCU_LINK(_trans, _dev) DOWNLINK_SEND_DEBUG_MCU_LINK(_trans, _dev, &link_mcu_nb_err, &link_mcu_fbw_nb_err, &mcu1_ppm_cpt);
Expand Down
15 changes: 12 additions & 3 deletions sw/airborne/firmwares/rotorcraft/telemetry.h
View file
Expand Up @@ -51,6 +51,12 @@
// I2C Error counters
#include "mcu_periph/i2c.h"

#if defined DOWNLINK
#define Downlink(x) x
#else
#define Downlink(x) {}
#endif

#define PERIODIC_SEND_ALIVE(_trans, _dev) DOWNLINK_SEND_ALIVE(_trans, _dev, 16, MD5SUM)

#include "subsystems/ins.h"
Expand All @@ -63,8 +69,8 @@
uint16_t vsup = electrical.vsupply; \
int16_t amps = (int16_t) (electrical.current/10); \
int16_t pwr = (int16_t) (vsup*amps); \
int16_t e = energy; \
DOWNLINK_SEND_ENERGY(_trans, _dev, &vsup, &amps, &pwr, &e, &v_ctl_throttle_slewed);\
int16_t e = 0; \
DOWNLINK_SEND_ENERGY(_trans, _dev, &vsup, &amps, &pwr, &e, 0);\
})

#define PERIODIC_SEND_ROTORCRAFT_STATUS(_trans, _dev) { \
Expand All @@ -91,7 +97,10 @@
}
#endif /*USE_GPS */

#define PERIODIC_SEND_MISSION_STATUS(_trans, _dev) SEND_MISSION_STATUS(_trans, _dev)
#define PERIODIC_SEND_MISSION_STATUS(_trans, _dev) Downlink({ \
SEND_MISSION_STATUS(_trans, _dev); \
})


#ifdef RADIO_CONTROL
#define PERIODIC_SEND_RC(_trans, _dev) DOWNLINK_SEND_RC(_trans, _dev, RADIO_CONTROL_NB_CHANNEL, radio_control.values)
Expand Down

0 comments on commit aa4faaa

Please sign in to comment.