Skip to content

Commit

Permalink
Implement smaller GPS messages from NATNET to PPRZ
Browse files Browse the repository at this point in the history 
closes #1308
  • Loading branch information
@LodewijkSikkel @flixr
LodewijkSikkel authored and flixr committed Sep 2, 2015
1 parent d463e02 commit 1cac358
Show file tree
Hide file tree
Showing 7 changed files with 433 additions and 17 deletions.
11 changes: 11 additions & 0 deletions conf/airframes/LS/conf.xml
View file
Expand Up @@ -10,6 +10,17 @@
settings_modules="modules/gps_ubx_ucenter.xml"
gui_color="#00000000ffff"
/>
<aircraft
name="BebopSmallGPSMes"
ac_id="1"
airframe="airframes/LS/quadrotor_bebop_small_gps_messages.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_optitrack.xml"
settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_int_quat.xml settings/control/stabilization_rate.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/control/rotorcraft_guidance.xml"
settings_modules="modules/geo_mag.xml modules/air_data.xml modules/video_rtp_stream.xml"
gui_color="blue"
/>
<aircraft
name="MavTecLS"
ac_id="150"
Expand Down
229 changes: 229 additions & 0 deletions conf/airframes/LS/quadrotor_bebop_small_gps_messages.xml
View file
@@ -0,0 +1,229 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">

<airframe name="bebop">

<firmware name="rotorcraft">
<target name="ap" board="bebop">
<configure name="USE_MAGNETOMETER" value="0"/> <!-- Disable magnetometer -->
</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="bebop"/>
<subsystem name="imu" type="bebop"/>
<subsystem name="gps" type="datalink"/>
<subsystem name="stabilization" type="int_quat"/>
<subsystem name="ahrs" type="int_cmpl_quat"/>
<subsystem name="ins" type="extended"/>
</firmware>

<modules main_freq="512">
<load name="geo_mag.xml"/>
<load name="air_data.xml"/>
<load name="send_imu_mag_current.xml"/>
<!--load name="logger_file.xml">
<define name="FILE_LOGGER_PATH" value="/data/ftp/internal_000"/>
</load-->
<load name="video_rtp_stream.xml">
<define name="VIEWVIDEO_FPS" value="60"/>
<define name="VIEWVIDEO_QUALITY_FACTOR" value="80"/>
<define name="VIEWVIDEO_DOWNSIZE_FACTOR" value="1"/>
<define name="VIEWVIDEO_DEVICE" value="/dev/video0"/>
<define name="VIEWVIDEO_SUBDEV" value="/dev/v4l-subdev0"/>
<define name="VIEWVIDEO_DEVICE_SIZE" value="320,240"/>
<define name="VIEWVIDEO_DEVICE_BUFFERS" value="60"/>
<define name="VIEWVIDEO_SHOT_PATH" value="/data/ftp/internal_000/images"/>
</load>
</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="6000"/>
</commands>

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

<section name="MIXING" prefix="MOTOR_MIXING_">
<!-- 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="AIR_DATA" prefix="AIR_DATA_">
<define name="CALC_AIRSPEED" value="FALSE"/>
<define name="CALC_TAS_FACTOR" value="FALSE"/>
<define name="CALC_AMSL_BARO" value="TRUE"/>
</section>

<section name="IMU" prefix="IMU_">
<!-- Magneto calibration -->
<define name="MAG_X_NEUTRAL" value="18"/>
<define name="MAG_Y_NEUTRAL" value="157"/>
<define name="MAG_Z_NEUTRAL" value="49"/>
<define name="MAG_X_SENS" value="10.5007722373" integer="16"/>
<define name="MAG_Y_SENS" value="11.1147400462" integer="16"/>
<define name="MAG_Z_SENS" value="11.6479371722" 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"/>
<define name="USE_GPS_HEADING" value="1"/>
</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="GPS" prefix="GPS_">
<define name="USE_DATALINK_SMALL" value="1"/>
<define name="LOCAL_ECEF_ORIGIN_X" value="392433200"/>
<define name="LOCAL_ECEF_ORIGIN_Y" value="30036200"/>
<define name="LOCAL_ECEF_ORIGIN_Z" value="500219700"/>
</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="300" unit="deg/s"/>
<define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="50"/>

<!-- 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="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.)"/>

<!-- feedback -->
<define name="PHI_PGAIN" value="2500"/>
<define name="PHI_DGAIN" value="300"/>
<define name="PHI_IGAIN" value="0"/>

<define name="THETA_PGAIN" value="2500"/>
<define name="THETA_DGAIN" value="300"/>
<define name="THETA_IGAIN" value="0"/>

<define name="PSI_PGAIN" value="8000"/>
<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="0"/>
</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="TRUE"/>
</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="100"/>
<define name="DGAIN" value="200"/>
<define name="IGAIN" value="15"/>
</section>

<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_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>

<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_NAV"/>
</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>
7 changes: 7 additions & 0 deletions conf/messages.xml
View file
Expand Up @@ -2451,6 +2451,13 @@
<field name="yaw" type="int8"/>
</message>

<message name="REMOTE_GPS_SMALL" id="54" link="broadcasted">
<field name="ac_id" type="uint8"/>
<field name="numsv" type="uint8"/>
<field name="pos_xyz" type="uint32" unit="cm"/>
<field name="speed_xy" type="uint32" unit="cm/s"/>
</message>

<message name="REMOTE_GPS" id="55" link="broadcasted">
<field name="ac_id" type="uint8"/>
<field name="numsv" type="uint8"/>
Expand Down
12 changes: 12 additions & 0 deletions sw/airborne/firmwares/rotorcraft/datalink.c
View file
Expand Up @@ -131,6 +131,18 @@ void dl_parse_msg(void)
break;
#endif // RADIO_CONTROL_TYPE_DATALINK
#if defined GPS_DATALINK
case DL_REMOTE_GPS_SMALL :
// Check if the GPS is for this AC
if (DL_REMOTE_GPS_SMALL_ac_id(dl_buffer) != AC_ID) {
break;
}

parse_gps_datalink_small(
DL_REMOTE_GPS_SMALL_numsv(dl_buffer),
DL_REMOTE_GPS_SMALL_pos_xyz(dl_buffer),
DL_REMOTE_GPS_SMALL_speed_xy(dl_buffer));
break;

case DL_REMOTE_GPS :
// Check if the GPS is for this AC
if (DL_REMOTE_GPS_ac_id(dl_buffer) != AC_ID) { break; }
Expand Down

0 comments on commit 1cac358

Please sign in to comment.