mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-04 05:42:49 +08:00
[separate FBW-AP] Transfer RC-channels from radio.h, send COMMANDS and FBW_STATUS from the ap side.
closes #726
This commit is contained in:
dewagter
Felix Ruess
@@ -0,0 +1,268 @@
|
|||||||
|
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
|
||||||
|
|
||||||
|
<!--
|
||||||
|
Use settings/tuning_ins.xml
|
||||||
|
Use telemetry/default_fixedwing_imu.xml
|
||||||
|
|
||||||
|
3x parallel redundant FBW-boards with DUAL-SBUS
|
||||||
|
1x Lisa-L with XSens Mtig700
|
||||||
|
-->
|
||||||
|
|
||||||
|
|
||||||
|
<airframe name="TwoSeasTwenty">
|
||||||
|
<!-- ************************* FIRMWARE ************************* -->
|
||||||
|
|
||||||
|
<firmware name="fixedwing">
|
||||||
|
<target name="ap" board="lisa_l_1.0">
|
||||||
|
<configure name="SEPARATE_FBW" value="1"/>
|
||||||
|
<configure name="AHRS_ALIGNER_LED" value="1"/>
|
||||||
|
<configure name="CPU_LED" value="1"/>
|
||||||
|
<define name="LINK_MCU_LED" value="4"/>
|
||||||
|
<!-- FBW -->
|
||||||
|
<subsystem name="actuators" type="dummy"/>
|
||||||
|
<subsystem name="intermcu" type="uart">
|
||||||
|
<configure name="INTERMCU_PORT_NR" value="2"/>
|
||||||
|
</subsystem>
|
||||||
|
<!-- AP -->
|
||||||
|
<subsystem name="control"/>
|
||||||
|
<subsystem name="navigation"/>
|
||||||
|
<!-- Communication -->
|
||||||
|
<subsystem name="telemetry" type="xbee_api">
|
||||||
|
<configure name="MODEM_BAUD" value="B9600"/>
|
||||||
|
<configure name="MODEM_PORT" value="UART3"/>
|
||||||
|
</subsystem>
|
||||||
|
<subsystem name="ins" type="xsens700">
|
||||||
|
<configure name="XSENS_UART_NR" value="1"/>
|
||||||
|
<configure name="XSENS_UART_BAUD" value="B230400"/>
|
||||||
|
</subsystem>
|
||||||
|
</target>
|
||||||
|
<target name="fbw" board="lisa_m_2.0">
|
||||||
|
<define name="OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP" value="1"/>
|
||||||
|
<define name="LINK_MCU_LED" value="1"/>
|
||||||
|
<configure name="HAS_LUFTBOOT" value="0"/>
|
||||||
|
<!-- no usb -->
|
||||||
|
<!-- RC -->
|
||||||
|
<subsystem name="radio_control" type="sbus_dual">
|
||||||
|
<configure name="SBUS1_PORT" value="UART3"/>
|
||||||
|
<configure name="SBUS2_PORT" value="UART5"/>
|
||||||
|
</subsystem>
|
||||||
|
<!-- FBW <-> AP -->
|
||||||
|
<subsystem name="intermcu" type="uart">
|
||||||
|
<configure name="INTERMCU_PORT_NR" value="1"/>
|
||||||
|
</subsystem>
|
||||||
|
<!-- SERVO'S -->
|
||||||
|
<subsystem name="actuators" type="pwm"/>
|
||||||
|
<define name="USE_SERVOS_7AND8"/>
|
||||||
|
</target>
|
||||||
|
<target name="sim" board="pc">
|
||||||
|
<subsystem name="radio_control" type="ppm"/>
|
||||||
|
<!-- AP -->
|
||||||
|
<subsystem name="control"/>
|
||||||
|
<subsystem name="navigation"/>
|
||||||
|
<!-- Communication -->
|
||||||
|
<subsystem name="telemetry" type="xbee_api">
|
||||||
|
<configure name="MODEM_BAUD" value="B9600"/>
|
||||||
|
<configure name="MODEM_PORT" value="UART3"/>
|
||||||
|
</subsystem>
|
||||||
|
<subsystem name="ins" type="xsens700">
|
||||||
|
<configure name="XSENS_UART_NR" value="1"/>
|
||||||
|
<configure name="XSENS_UART_BAUD" value="B230400"/>
|
||||||
|
</subsystem>
|
||||||
|
</target>
|
||||||
|
<define name="AGR_CLIMB"/>
|
||||||
|
<define name="LOITER_TRIM"/>
|
||||||
|
<define name="TUNE_AGRESSIVE_CLIMB"/>
|
||||||
|
<define name="STRONG_WIND"/>
|
||||||
|
<define name="WIND_INFO"/>
|
||||||
|
<define name="WIND_INFO_RET"/>
|
||||||
|
<define name="RADIO_CONTROL_AUTO1"/>
|
||||||
|
</firmware>
|
||||||
|
|
||||||
|
<!-- ************************* MODULES ************************* -->
|
||||||
|
|
||||||
|
<modules>
|
||||||
|
<load name="light.xml">
|
||||||
|
<define name="LIGHT_LED_STROBE" value="6"/>
|
||||||
|
<!-- <define name="LIGHT_LED_NAV" value="2"/> -->
|
||||||
|
<define name="STROBE_LIGHT_MODE_DEFAULT" value="6"/>
|
||||||
|
<define name="NAV_LIGHT_MODE_DEFAULT" value="4"/>
|
||||||
|
</load>
|
||||||
|
<load name="nav_line.xml"/>
|
||||||
|
<!--load name="gas_engine_idle.xml"/-->
|
||||||
|
</modules>
|
||||||
|
|
||||||
|
<!-- ************************* ACTUATORS ************************* -->
|
||||||
|
|
||||||
|
<servos>
|
||||||
|
<servo name="THROTTLE_LEFT" no="3" min="1950" neutral="1850" max="900"/>
|
||||||
|
<servo name="THROTTLE_RIGHT" no="2" min="1950" neutral="1850" max="900"/>
|
||||||
|
<servo name="AILERONS" no="1" min="1800" neutral="1500" max="1120"/>
|
||||||
|
<servo name="ELEVATORS" no="0" max="1000" neutral="1455" min="1900"/>
|
||||||
|
<servo name="RUDDER" no="4" max="1875" neutral="1555" min="1200"/>
|
||||||
|
<servo name="FLAPS" no="5" max="1020" neutral="1335" min="1650"/>
|
||||||
|
</servos>
|
||||||
|
|
||||||
|
<commands>
|
||||||
|
<axis name="THROTTLE" failsafe_value="0"/>
|
||||||
|
<axis name="ROLL" failsafe_value="0"/>
|
||||||
|
<axis name="PITCH" failsafe_value="0"/>
|
||||||
|
<axis name="YAW" failsafe_value="0"/>
|
||||||
|
<axis name="FLAPS" failsafe_value="-9600"/>
|
||||||
|
<axis name="IDLE1" failsafe_value="0"/>
|
||||||
|
<axis name="IDLE2" failsafe_value="0"/>
|
||||||
|
</commands>
|
||||||
|
|
||||||
|
<rc_commands>
|
||||||
|
<set command="THROTTLE" value="@THROTTLE"/>
|
||||||
|
<set command="ROLL" value="@ROLL"/>
|
||||||
|
<set command="PITCH" value="@PITCH"/>
|
||||||
|
<set command="YAW" value="@YAW"/>
|
||||||
|
<set command="FLAPS" value="@FLAPS"/>
|
||||||
|
<set command="IDLE1" value="@GAIN1"/>
|
||||||
|
<set command="IDLE2" value="@GAIN2"/>
|
||||||
|
</rc_commands>
|
||||||
|
|
||||||
|
<section name="AUTO1" prefix="AUTO1_">
|
||||||
|
<define name="MAX_ROLL" value="70" unit="deg"/>
|
||||||
|
<define name="MAX_PITCH" value="50" unit="deg"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="SERVO_MIXER_GAINS">
|
||||||
|
<define name="MAX_BRAKE_RATE" value="80"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<command_laws>
|
||||||
|
<ratelimit var="slow_flaps" value="@FLAPS" rate_min="-MAX_BRAKE_RATE" rate_max="MAX_BRAKE_RATE"/>
|
||||||
|
<set servo="THROTTLE_LEFT" value="((@THROTTLE)*0.9f) - ((@IDLE1)*0.1f)"/>
|
||||||
|
<set servo="THROTTLE_RIGHT" value="((@THROTTLE)*0.9f) - ((@IDLE2)*0.1f)"/>
|
||||||
|
<set servo="AILERONS" value="@ROLL"/>
|
||||||
|
<set servo="ELEVATORS" value="@PITCH"/>
|
||||||
|
<set servo="RUDDER" value="@YAW"/>
|
||||||
|
<set servo="FLAPS" value="$slow_flaps"/>
|
||||||
|
</command_laws>
|
||||||
|
|
||||||
|
<section name="FAILSAFE" prefix="FAILSAFE_">
|
||||||
|
<define name="DELAY_WITHOUT_GPS" value="1" unit="s"/>
|
||||||
|
<define name="DEFAULT_THROTTLE" value="0.0" unit="%"/>
|
||||||
|
<define name="DEFAULT_ROLL" value="0.0" unit="rad"/>
|
||||||
|
<define name="DEFAULT_PITCH" value="0.0" unit="rad"/>
|
||||||
|
<define name="HOME_RADIUS" value="100" unit="m"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<!-- ************************* SENSORS ************************* -->
|
||||||
|
|
||||||
|
<!--
|
||||||
|
<section name="XSENS">
|
||||||
|
<define name="GPS_IMU_LEVER_ARM_X" value="-0.285f"/>
|
||||||
|
<define name="GPS_IMU_LEVER_ARM_Y" value="0.0f"/>
|
||||||
|
<define name="GPS_IMU_LEVER_ARM_Z" value="0.0f"/>
|
||||||
|
</section>
|
||||||
|
-->
|
||||||
|
|
||||||
|
<section name="IMU" prefix="IMU_">
|
||||||
|
<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>
|
||||||
|
|
||||||
|
<section name="INS" prefix="INS_">
|
||||||
|
<define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
|
||||||
|
<define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<!-- ************************* GAINS ************************* -->
|
||||||
|
|
||||||
|
<section name="HORIZONTAL CONTROL" prefix="H_CTL_">
|
||||||
|
<define name="COURSE_PGAIN" value="0.85"/>
|
||||||
|
<define name="ROLL_MAX_SETPOINT" value="0.6" unit="rad"/>
|
||||||
|
<define name="PITCH_MAX_SETPOINT" value="0.5" unit="rad"/>
|
||||||
|
<define name="PITCH_MIN_SETPOINT" value="-0.5" unit="rad"/>
|
||||||
|
<!--
|
||||||
|
<define name="ROLL_PGAIN" value="0."/>
|
||||||
|
<define name="AILERON_OF_THROTTLE" value="0.0"/>
|
||||||
|
<define name="PITCH_PGAIN" value="0."/>
|
||||||
|
<define name="PITCH_DGAIN" value="0"/>
|
||||||
|
<define name="ELEVATOR_OF_ROLL" value="0"/>
|
||||||
|
-->
|
||||||
|
<define name="ROLL_PGAIN" value="12000."/>
|
||||||
|
<define name="AILERON_OF_THROTTLE" value="0.0"/>
|
||||||
|
<define name="PITCH_PGAIN" value="9000."/>
|
||||||
|
<define name="PITCH_DGAIN" value="1.5"/>
|
||||||
|
<define name="ELEVATOR_OF_ROLL" value="1250"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="VERTICAL CONTROL" prefix="V_CTL_">
|
||||||
|
<define name="POWER_CTL_BAT_NOMINAL" value="7.6" unit="volt"/>
|
||||||
|
<!-- outer loop proportional gain -->
|
||||||
|
<define name="ALTITUDE_PGAIN" value="0.07"/>
|
||||||
|
<!-- outer loop saturation -->
|
||||||
|
<define name="ALTITUDE_MAX_CLIMB" value="2."/>
|
||||||
|
<!-- auto throttle inner loop -->
|
||||||
|
<define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.55"/>
|
||||||
|
<define name="AUTO_THROTTLE_MIN_CRUISE_THROTTLE" value="0.30"/>
|
||||||
|
<define name="AUTO_THROTTLE_MAX_CRUISE_THROTTLE" value="0.80"/>
|
||||||
|
<define name="AUTO_THROTTLE_LOITER_TRIM" value="1500"/>
|
||||||
|
<define name="AUTO_THROTTLE_DASH_TRIM" value="-500"/>
|
||||||
|
<define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.15" unit="%/(m/s)"/>
|
||||||
|
<define name="AUTO_THROTTLE_PGAIN" value="0.025"/>
|
||||||
|
<define name="AUTO_THROTTLE_IGAIN" value="0.1"/>
|
||||||
|
<define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.05"/>
|
||||||
|
<!-- auto pitch inner loop -->
|
||||||
|
<define name="AUTO_PITCH_PGAIN" value="0.05"/>
|
||||||
|
<define name="AUTO_PITCH_IGAIN" value="0.075"/>
|
||||||
|
<define name="AUTO_PITCH_MAX_PITCH" value="0.35"/>
|
||||||
|
<define name="AUTO_PITCH_MIN_PITCH" value="-0.35"/>
|
||||||
|
<define name="THROTTLE_SLEW" value="0.05"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="AGGRESSIVE" prefix="AGR_">
|
||||||
|
<define name="BLEND_START" value="20"/>
|
||||||
|
<!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
|
||||||
|
<define name="BLEND_END" value="10"/>
|
||||||
|
<!-- Altitude Error to Blend Aggressive to Regular Climb Modes CANNOT BE ZERO!!-->
|
||||||
|
<define name="CLIMB_THROTTLE" value="0.8"/>
|
||||||
|
<!-- Gaz for Aggressive Climb -->
|
||||||
|
<define name="CLIMB_PITCH" value="0.3"/>
|
||||||
|
<!-- Pitch for Aggressive Climb -->
|
||||||
|
<define name="DESCENT_THROTTLE" value="0.1"/>
|
||||||
|
<!-- Gaz for Aggressive Decent -->
|
||||||
|
<define name="DESCENT_PITCH" value="-0.25"/>
|
||||||
|
<!-- Pitch for Aggressive Decent -->
|
||||||
|
<define name="CLIMB_NAV_RATIO" value="0.8"/>
|
||||||
|
<!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
|
||||||
|
<define name="DESCENT_NAV_RATIO" value="1.0"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<!-- ************************* MISC ************************* -->
|
||||||
|
|
||||||
|
<section name="BAT">
|
||||||
|
<!-- 2S LiPo with 1000mAh -->
|
||||||
|
<define name="LOW_BAT_LEVEL" value="7.5" unit="V"/>
|
||||||
|
<define name="CRITIC_BAT_LEVEL" value="7.3" unit="V"/>
|
||||||
|
<define name="CATASTROPHIC_BAT_LEVEL" value="7.0" unit="V"/>
|
||||||
|
<define name="MAX_BAT_LEVEL" value="8.4" unit="V"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="SIMU">
|
||||||
|
<define name="YAW_RESPONSE_FACTOR" value="0.7"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="MISC">
|
||||||
|
<define name="TELEMETRY_MODE_FBW" value="1"/>
|
||||||
|
<define name="NOMINAL_AIRSPEED" value="28.0" unit="m/s"/>
|
||||||
|
<define name="CARROT" value="5." unit="s"/>
|
||||||
|
<define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
|
||||||
|
<define name="DEFAULT_CIRCLE_RADIUS" value="80."/>
|
||||||
|
<define name="XBEE_INIT" value=""ATPL4\rATRN5\rATTT80\r""/>
|
||||||
|
<!-- <define name="NO_XBEE_API_INIT" value="TRUE"/> -->
|
||||||
|
<define name="COMMAND_ROLL_TRIM" value="180"/>
|
||||||
|
<define name="COMMAND_PITCH_TRIM" value="-194."/>
|
||||||
|
<define name="VoltageOfAdc(adc)" value="(0.0034912109375*adc)"/>
|
||||||
|
<!-- 12bit 3.3V over 10k/3k bridge -->
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="NAV">
|
||||||
|
<define name="NAV_GLIDE_PITCH_TRIM" value="0"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
</airframe>
|
||||||
@@ -0,0 +1,12 @@
|
|||||||
|
<conf>
|
||||||
|
<aircraft
|
||||||
|
name="TwoSeas"
|
||||||
|
ac_id="2"
|
||||||
|
airframe="airframes/CDW/TwoSeas.xml"
|
||||||
|
radio="radios/T8FG_SBUS.xml"
|
||||||
|
telemetry="telemetry/default_fixedwing.xml"
|
||||||
|
flight_plan="flight_plans/versatile.xml"
|
||||||
|
settings=" settings/fixedwing_basic.xml settings/control/ctl_basic.xml settings/estimation/ins_neutrals.xml"
|
||||||
|
gui_color="blue"
|
||||||
|
/>
|
||||||
|
</conf>
|
||||||
@@ -28,6 +28,8 @@
|
|||||||
<message name="GPS_SOL" period="2.0"/>
|
<message name="GPS_SOL" period="2.0"/>
|
||||||
<message name="CAM" period="0.5"/>
|
<message name="CAM" period="0.5"/>
|
||||||
<message name="CAM_POINT" period="1.0"/>
|
<message name="CAM_POINT" period="1.0"/>
|
||||||
|
<message name="COMMANDS" period="5"/>
|
||||||
|
<message name="FBW_STATUS" period="2"/>
|
||||||
</mode>
|
</mode>
|
||||||
<mode name="minimal">
|
<mode name="minimal">
|
||||||
<message name="ALIVE" period="5"/>
|
<message name="ALIVE" period="5"/>
|
||||||
|
|||||||
@@ -30,6 +30,8 @@
|
|||||||
<message name="IMU_MAG" period="1.3"/>
|
<message name="IMU_MAG" period="1.3"/>
|
||||||
<message name="CAM" period="0.5"/>
|
<message name="CAM" period="0.5"/>
|
||||||
<message name="CAM_POINT" period="1.0"/>
|
<message name="CAM_POINT" period="1.0"/>
|
||||||
|
<message name="COMMANDS" period="5"/>
|
||||||
|
<message name="FBW_STATUS" period="2"/>
|
||||||
</mode>
|
</mode>
|
||||||
<mode name="minimal">
|
<mode name="minimal">
|
||||||
<message name="ALIVE" period="5"/>
|
<message name="ALIVE" period="5"/>
|
||||||
|
|||||||
@@ -28,6 +28,8 @@
|
|||||||
<message name="IMU_ACCEL" period=".8"/>
|
<message name="IMU_ACCEL" period=".8"/>
|
||||||
<message name="IMU_GYRO" period=".9"/>
|
<message name="IMU_GYRO" period=".9"/>
|
||||||
<message name="IMU_MAG" period="1.9"/>
|
<message name="IMU_MAG" period="1.9"/>
|
||||||
|
<message name="COMMANDS" period="5"/>
|
||||||
|
<message name="FBW_STATUS" period="2"/>
|
||||||
</mode>
|
</mode>
|
||||||
<mode name="minimal">
|
<mode name="minimal">
|
||||||
<message name="ALIVE" period="5"/>
|
<message name="ALIVE" period="5"/>
|
||||||
|
|||||||
@@ -244,11 +244,50 @@ struct link_mcu_msg link_mcu_from_fbw_msg;
|
|||||||
|
|
||||||
inline void parse_mavpilot_msg( void );
|
inline void parse_mavpilot_msg( void );
|
||||||
|
|
||||||
|
|
||||||
|
#ifdef AP
|
||||||
|
#include "subsystems/datalink/telemetry.h"
|
||||||
|
|
||||||
|
#define RC_OK 0
|
||||||
|
#define RC_LOST 1
|
||||||
|
#define RC_REALLY_LOST 2
|
||||||
|
|
||||||
|
|
||||||
|
static void send_commands(void) {
|
||||||
|
DOWNLINK_SEND_COMMANDS(DefaultChannel, DefaultDevice, COMMANDS_NB, ap_state->commands);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
static void send_fbw_status(void) {
|
||||||
|
uint8_t rc_status = 0;
|
||||||
|
uint8_t fbw_status = 0;
|
||||||
|
if (fbw_state->status & _BV(STATUS_MODE_AUTO))
|
||||||
|
fbw_status = FBW_MODE_AUTO;
|
||||||
|
if (fbw_state->status & _BV(STATUS_MODE_FAILSAFE))
|
||||||
|
fbw_status = FBW_MODE_FAILSAFE;
|
||||||
|
if (fbw_state->status & _BV(STATUS_RADIO_REALLY_LOST))
|
||||||
|
rc_status = RC_REALLY_LOST;
|
||||||
|
else if (fbw_state->status & _BV(RC_OK))
|
||||||
|
rc_status = RC_OK;
|
||||||
|
else
|
||||||
|
rc_status = RC_LOST;
|
||||||
|
DOWNLINK_SEND_FBW_STATUS(DefaultChannel, DefaultDevice,
|
||||||
|
&(rc_status), &(fbw_state->ppm_cpt), &(fbw_status), &(fbw_state->vsupply), &(fbw_state->current));
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
void link_mcu_init( void )
|
void link_mcu_init( void )
|
||||||
{
|
{
|
||||||
intermcu_data.status = LINK_MCU_UNINIT;
|
intermcu_data.status = LINK_MCU_UNINIT;
|
||||||
intermcu_data.msg_available = FALSE;
|
intermcu_data.msg_available = FALSE;
|
||||||
intermcu_data.error_cnt = 0;
|
intermcu_data.error_cnt = 0;
|
||||||
|
#ifdef AP
|
||||||
|
#if PERIODIC_TELEMETRY
|
||||||
|
// If FBW has not telemetry, then AP can send some of the info
|
||||||
|
register_periodic_telemetry(DefaultPeriodic, "COMMANDS", send_commands);
|
||||||
|
register_periodic_telemetry(DefaultPeriodic, "FBW_STATUS", send_fbw_status);
|
||||||
|
#endif
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
void parse_mavpilot_msg( void )
|
void parse_mavpilot_msg( void )
|
||||||
|
|||||||
Reference in New Issue
Block a user