Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-27 17:06:31 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

[nps] started fixing nps sim for rotorcrafts

Browse Source
This commit is contained in:
Felix Ruess
2012-05-29 18:26:49 +02:00
parent fd2d1ecc8a
commit ba701ec564
11 changed files with 532 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/examples/quadrotor_lisa_m_2_pwm.xml
+5 -22
View File
@@ -5,15 +5,11 @@
<airframe name="fraser">
<firmware name="rotorcraft">
<target name="ap" board="lisa_m_2.0">
<subsystem name="actuators" type="pwm_supervision">
<define name="SERVO_HZ" value="400"/>
</subsystem>
</target>
<target name="sim" board="pc">
<subsystem name="fdm" type="nps"/>
<subsystem name="actuators" type="mkk"/>
</target>
<target name="ap" board="lisa_m_2.0"/>
<subsystem name="actuators" type="pwm_supervision">
<define name="SERVO_HZ" value="400"/>
</subsystem>
<subsystem name="radio_control" type="ppm"/>
<subsystem name="telemetry" type="transparent"/>
@@ -42,13 +38,6 @@
<!-- but can be empty if no additional servos are used -->
</command_laws>
<!-- only for the nps sim -->
<section name="ACTUATORS_MKK" prefix="ACTUATORS_MKK_">
<define name="NB" value="4"/>
<!-- FRONT, BACK, RIGHT, LEFT -->
<define name="ADDR" value="{ 0x52, 0x54, 0x56, 0x58 }"/>
</section>
<section name="SUPERVISION" prefix="SUPERVISION_">
<define name="STOP_MOTOR" value="1000"/>
<define name="MIN_MOTOR" value="1100"/>
@@ -186,12 +175,6 @@
<define name="IGAIN" value="0"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
<define name="INITIAL_CONDITITONS" value="&quot;reset00&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_aspirin.h&quot;"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
conf/airframes/examples/quadrotor_lisa_m_mkk.xml
+1 -1
View File
@@ -172,7 +172,7 @@
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
<define name="INITIAL_CONDITITONS" value="&quot;reset00&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_aspirin.h&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_aspirin_1.5.h&quot;"/>
</section>
<section name="AUTOPILOT">
conf/airframes/fraser_lisa_m_rotorcraft.xml
+1 -1
View File
@@ -208,7 +208,7 @@
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
<define name="INITIAL_CONDITITONS" value="&quot;reset00&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_aspirin.h&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_aspirin_1.5.h&quot;"/>
</section>
<section name="AUTOPILOT">
conf/simulator/jsbsim/aircraft/Quad_LisaM.xml
+396
View File
@@ -0,0 +1,396 @@
<?xml version="1.0"?>
<?xml-stylesheet type="text/xsl" href="http://jsbsim.sourceforge.net/JSBSim.xsl"?>
<fdm_config name="QUAD COMPLETE EXT" version="2.0" release="BETA" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="http://jsbsim.sourceforge.net/JSBSim.xsd">
<fileheader>
<author> Gustavo Violato &amp; Antoine Drouin</author>
<filecreationdate> 24-02-2009 </filecreationdate>
<version> Version 0.9 - beta</version>
<description> Simple Quadrotor without rotor dynamic </description>
</fileheader>
<metrics>
<wingarea unit="IN2"> 78.53 </wingarea>
<wingspan unit="IN"> 10 </wingspan>
<chord unit="IN"> 6.89 </chord>
<htailarea unit="FT2"> 0 </htailarea>
<htailarm unit="FT"> 0 </htailarm>
<vtailarea unit="FT2"> 0 </vtailarea>
<vtailarm unit="FT"> 0 </vtailarm>
<location name="AERORP" unit="IN">
<x> 0 </x>
<y> 0 </y>
<z> 0 </z>
</location>
<location name="EYEPOINT" unit="IN">
<x> 0 </x>
<y> 0 </y>
<z> 0 </z>
</location>
<location name="VRP" unit="IN">
<x> 0 </x>
<y> 0 </y>
<z> 0 </z>
</location>
</metrics>
<mass_balance>
<ixx unit="SLUG*FT2"> 0.005 </ixx>
<iyy unit="SLUG*FT2"> 0.005 </iyy>
<izz unit="SLUG*FT2"> 0.010 </izz>
<ixy unit="SLUG*FT2"> 0. </ixy>
<ixz unit="SLUG*FT2"> 0. </ixz>
<iyz unit="SLUG*FT2"> 0. </iyz>
<emptywt unit="LBS"> 0.84 </emptywt>
<location name="CG" unit="M">
<x> 0 </x>
<y> 0 </y>
<z> 0 </z>
</location>
</mass_balance>
<ground_reactions>
<contact type="STRUCTURE" name="CONTACT_FRONT">
<location unit="M">
<x>-0.15 </x>
<y> 0 </y>
<z>-0.1 </z>
</location>
<static_friction> 0.8 </static_friction>
<dynamic_friction> 0.5 </dynamic_friction>
<spring_coeff unit="N/M"> 500 </spring_coeff>
<damping_coeff unit="N/M/SEC"> 100 </damping_coeff>
<damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound>
<max_steer unit="DEG"> 0.0 </max_steer>
<brake_group> NONE </brake_group>
<retractable>0</retractable>
</contact>
<contact type="STRUCTURE" name="CONTACT_BACK">
<location unit="M">
<x> 0.15</x>
<y> 0 </y>
<z>-0.1 </z>
</location>
<static_friction> 0.8 </static_friction>
<dynamic_friction> 0.5 </dynamic_friction>
<spring_coeff unit="N/M"> 500 </spring_coeff>
<damping_coeff unit="N/M/SEC"> 100 </damping_coeff>
<damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound>
<max_steer unit="DEG"> 0.0 </max_steer>
<brake_group> NONE </brake_group>
<retractable>0</retractable>
</contact>
<contact type="STRUCTURE" name="CONTACT_RIGHT">
<location unit="M">
<x> 0. </x>
<y> 0.15</y>
<z>-0.1 </z>
</location>
<static_friction> 0.8 </static_friction>
<dynamic_friction> 0.5 </dynamic_friction>
<spring_coeff unit="N/M"> 500 </spring_coeff>
<damping_coeff unit="N/M/SEC"> 100 </damping_coeff>
<damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound>
<max_steer unit="DEG"> 0.0 </max_steer>
<brake_group> NONE </brake_group>
<retractable>0</retractable>
</contact>
<contact type="STRUCTURE" name="CONTACT_LEFT">
<location unit="M">
<x> 0. </x>
<y>-0.15</y>
<z>-0.1 </z>
</location>
<static_friction> 0.8 </static_friction>
<dynamic_friction> 0.5 </dynamic_friction>
<spring_coeff unit="N/M"> 500 </spring_coeff>
<damping_coeff unit="N/M/SEC"> 100 </damping_coeff>
<damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound>
<max_steer unit="DEG"> 0.0 </max_steer>
<brake_group> NONE </brake_group>
<retractable>0</retractable>
</contact>
</ground_reactions>
<external_reactions>
<property>fcs/front_motor</property>
<property>fcs/back_motor</property>
<property>fcs/right_motor</property>
<property>fcs/left_motor</property>
<!-- First the lift forces produced by each propeller -->
<force name="front_motor" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/front_motor</property>
<value> 0.84 </value>
</product>
</function>
<location unit="IN">
<x>-6.89</x>
<y>0</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>0</y>
<z>-1</z>
</direction>
</force>
<force name="back_motor" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/back_motor</property>
<value> 0.84 </value>
</product>
</function>
<location unit="IN">
<x>6.89</x>
<y>0</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>0</y>
<z>-1</z>
</direction>
</force>
<force name="right_motor" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/right_motor</property>
<value> 0.84 </value>
</product>
</function>
<location unit="IN">
<x>0</x>
<y>6.89</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>0</y>
<z>-1</z>
</direction>
</force>
<force name="left_motor" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/left_motor</property>
<value> 0.84 </value>
</product>
</function>
<location unit="IN">
<x>0</x>
<y>-6.89</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>0</y>
<z>-1</z>
</direction>
</force>
<!-- Then the Moment Couples -->
<!-- Front Engine -->
<force name="front_couple1" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/front_motor</property>
<value> 0.84 </value>
</product>
</function>
<location unit="IN">
<x>-6.89</x>
<!-- Necessary arm in IN to produce a moment ten times
"weaker" then the force when both are measured in the SI.
front and back motors turning clockwise, left and right motors
turning anti-clockwise when view from up-->
<y>1.9685</y>
<z>0</z>
</location>
<direction>
<x>1</x>
<y>0</y>
<z>0</z>
</direction>
</force>
<force name="front_couple2" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/front_motor</property>
<value> 0.84 </value>
</product>
</function>
<location unit="IN">
<x>-6.89</x>
<y>-1.9685</y>
<z>0</z>
</location>
<direction>
<x>-1</x>
<y>0</y>
<z>0</z>
</direction>
</force>
<!-- Back Engine -->
<force name="back_couple1" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/back_motor</property>
<value> 0.84 </value>
</product>
</function>
<location unit="IN">
<x>6.89</x>
<y>1.9685</y>
<z>0</z>
</location>
<direction>
<x>1</x>
<y>0</y>
<z>0</z>
</direction>
</force>
<force name="back_couple2" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/back_motor</property>
<value> 0.84 </value>
</product>
</function>
<location unit="IN">
<x>6.89</x>
<y>-1.9685</y>
<z>0</z>
</location>
<direction>
<x>-1</x>
<y>0</y>
<z>0</z>
</direction>
</force>
<!-- Right Engine -->
<force name="right_couple1" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/right_motor</property>
<value> 0.84 </value>
</product>
</function>
<location unit="IN">
<x>-1.9685</x>
<y>6.89</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>1</y>
<z>0</z>
</direction>
</force>
<force name="right_couple2" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/right_motor</property>
<value> 0.84 </value>
</product>
</function>
<location unit="IN">
<x>1.9685</x>
<y>6.89</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>-1</y>
<z>0</z>
</direction>
</force>
<!-- Left Engine -->
<force name="left_couple1" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/left_motor</property>
<value> 0.84 </value>
</product>
</function>
<location unit="IN">
<x>-1.9685</x>
<y>-6.89</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>1</y>
<z>0</z>
</direction>
</force>
<force name="left_couple2" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/left_motor</property>
<value> 0.84 </value>
</product>
</function>
<location unit="IN">
<x>1.9685</x>
<y>-6.89</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>-1</y>
<z>0</z>
</direction>
</force>
</external_reactions>
<propulsion/>
<flight_control name="FGFCS"/>
<aerodynamics>
<axis name="DRAG">
<function name="aero/coefficient/CD">
<description>Drag</description>
<product>
<property>aero/qbar-psf</property>
<value>47.9</value> <!-- Conversion to pascals -->
<value>0.0151</value> <!-- CD x Area (m^2) -->
<value>0.224808943</value> <!-- N to LBS -->
</product>
</function>
</axis>
</aerodynamics>
</fdm_config>
conf/simulator/nps/nps_sensors_params_aspirin.h → conf/simulator/nps/nps_sensors_params_aspirin_1.5.h
+11 -13
View File
@@ -1,7 +1,5 @@
/*
* $Id$
*
* Copyright (C) 2009 Antoine Drouin
* Copyright (C) 2012 Felix Ruess <felix.ruess@gmail.com>
*
* This file is part of paparazzi.
*
@@ -19,13 +17,13 @@
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
#ifndef NPS_SENSORS_PARAMS_H
#define NPS_SENSORS_PARAMS_H
#include "generated/airframe.h"
#include "subsystems/imu.h"
#if 1
#define NPS_BODY_TO_IMU_PHI IMU_BODY_TO_IMU_PHI
@@ -104,17 +102,17 @@
#define NPS_MAG_MIN -2047
#define NPS_MAG_MAX 2047
#define NPS_MAG_IMU_TO_SENSOR_PHI 0.
#define NPS_MAG_IMU_TO_SENSOR_THETA 0.
#define NPS_MAG_IMU_TO_SENSOR_PSI RadOfDeg(45.)
#define NPS_MAG_IMU_TO_SENSOR_PHI 0.
#define NPS_MAG_IMU_TO_SENSOR_THETA 0.
#define NPS_MAG_IMU_TO_SENSOR_PSI 0.
#define NPS_MAG_SENSITIVITY_XX MAG_BFP_OF_REAL(-1./4.94075530)
#define NPS_MAG_SENSITIVITY_YY MAG_BFP_OF_REAL( 1./5.10207664)
#define NPS_MAG_SENSITIVITY_ZZ MAG_BFP_OF_REAL(-1./4.90788848)
#define NPS_MAG_SENSITIVITY_XX IMU_MAG_X_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_X_SENS)
#define NPS_MAG_SENSITIVITY_YY IMU_MAG_Y_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_Y_SENS)
#define NPS_MAG_SENSITIVITY_ZZ IMU_MAG_Z_SIGN * MAG_BFP_OF_REAL(1./IMU_MAG_Z_SENS)
#define NPS_MAG_NEUTRAL_X 2358
#define NPS_MAG_NEUTRAL_Y 2362
#define NPS_MAG_NEUTRAL_Z 2119
#define NPS_MAG_NEUTRAL_X IMU_MAG_X_NEUTRAL
#define NPS_MAG_NEUTRAL_Y IMU_MAG_Y_NEUTRAL
#define NPS_MAG_NEUTRAL_Z IMU_MAG_Z_NEUTRAL
#define NPS_MAG_NOISE_STD_DEV_X 2e-3
#define NPS_MAG_NOISE_STD_DEV_Y 2e-3
sw/simulator/nps/nps_autopilot_rotorcraft.c
+31 -5
View File
@@ -1,3 +1,24 @@
/*
* Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#include "nps_autopilot_rotorcraft.h"
#include "firmwares/rotorcraft/main.h"
@@ -21,7 +42,6 @@ void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, cha
nps_radio_control_init(type_rc, num_rc_script, rc_dev);
nps_bypass_ahrs = TRUE;
// nps_bypass_ahrs = FALSE;
main_init();
@@ -73,16 +93,22 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) {
handle_periodic_tasks();
if (time < 8) { /* start with a little bit of hovering */
/* override the commands for the first 8 seconds...
* start with a little bit of hovering
*/
if (time < 8) {
int32_t init_cmd[4];
init_cmd[COMMAND_THRUST] = 0.253*SUPERVISION_MAX_MOTOR;
init_cmd[COMMAND_THRUST] = 0.253*MAX_PPRZ;
init_cmd[COMMAND_ROLL] = 0;
init_cmd[COMMAND_PITCH] = 0;
init_cmd[COMMAND_YAW] = 0;
supervision_run(TRUE, FALSE, init_cmd);
}
for (uint8_t i=0; i<ACTUATORS_MKK_NB; i++)
autopilot.commands[i] = (double)supervision.commands[i] / SUPERVISION_MAX_MOTOR;
/* scale final motor commands to 0-1 for feeding the fdm */
/* FIXME: autopilot.commands is of length NB_COMMANDS instead of number of motors */
for (uint8_t i=0; i<SUPERVISION_NB_MOTOR; i++)
autopilot.commands[i] = (double)(supervision.commands[i] - SUPERVISION_MIN_MOTOR) / SUPERVISION_MAX_MOTOR;
}
sw/simulator/nps/nps_autopilot_rotorcraft.h
+3 -3
View File
@@ -1,5 +1,5 @@
#ifndef NPS_AUTOPILOT_BOOZ_H
#define NPS_AUTOPILOT_BOOZ_H
#ifndef NPS_AUTOPILOT_ROTORCRAFT_H
#define NPS_AUTOPILOT_ROTORCRAFT_H
#include "nps_autopilot.h"
@@ -7,4 +7,4 @@
extern bool_t nps_bypass_ahrs;
extern void sim_overwrite_ahrs(void);
#endif /* NPS_AUTOPILOT_BOOZ_H */
#endif /* NPS_AUTOPILOT_ROTORCRAFT_H */
sw/simulator/nps/nps_fdm.h
+21
View File
@@ -1,3 +1,24 @@
/*
* Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#ifndef NPS_FDM
#define NPS_FDM
sw/simulator/nps/nps_fdm_jsbsim.c
+21
View File
@@ -1,3 +1,24 @@
/*
* Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#include <FGFDMExec.h>
#include <FGJSBBase.h>
#include <models/FGPropulsion.h>
sw/simulator/nps/nps_radio_control.c
+21
View File
@@ -1,3 +1,24 @@
/*
* Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#include "nps_radio_control.h"
#include "nps_radio_control_spektrum.h"
sw/simulator/nps/nps_radio_control.h
+21
View File
@@ -1,3 +1,24 @@
/*
* Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
#ifndef NPS_RADIO_CONTROL_H
#define NPS_RADIO_CONTROL_H