Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
nps_autopilot_fixedwing.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
3  * Copyright (C) 2013 The Paparazzi Team
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
23 #include "nps_autopilot.h"
24 
25 #ifdef FBW
27 #define Fbw(f) f ## _fbw()
28 #else
29 #define Fbw(f)
30 #endif
31 
32 #ifdef AP
34 #define Ap(f) f ## _ap()
35 #else
36 #define Ap(f)
37 #endif
38 
39 #include "nps_sensors.h"
40 #include "nps_radio_control.h"
41 #include "nps_electrical.h"
42 #include "nps_fdm.h"
43 
45 #include "subsystems/imu.h"
46 #include "mcu_periph/sys_time.h"
47 #include "state.h"
48 #include "subsystems/commands.h"
49 
50 #include "subsystems/abi.h"
51 
52 // for launch
54 
55 // for datalink_time hack
57 
61 
62 #ifndef NPS_BYPASS_AHRS
63 #define NPS_BYPASS_AHRS FALSE
64 #endif
65 
66 #ifndef NPS_BYPASS_INS
67 #define NPS_BYPASS_INS FALSE
68 #endif
69 
70 
71 #if !defined (FBW) || !defined (AP)
72 #error NPS does not currently support dual processor simulation for FBW and AP on fixedwing!
73 #endif
74 
75 void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char *rc_dev)
76 {
77 
79 
80  nps_radio_control_init(type_rc, num_rc_script, rc_dev);
82 
85 
86  Fbw(init);
87  Ap(init);
88 
89 }
90 
92 {
94 }
95 
96 #include <stdio.h>
97 #include "subsystems/gps.h"
98 
99 void nps_autopilot_run_step(double time)
100 {
101 
103 
104 #if RADIO_CONTROL && !RADIO_CONTROL_TYPE_DATALINK
105  if (nps_radio_control_available(time)) {
106  radio_control_feed();
107  Fbw(event_task);
108  }
109 #endif
110 
113  Fbw(event_task);
114  Ap(event_task);
115  }
116 
118  imu_feed_mag();
119  Fbw(event_task);
120  Ap(event_task);
121  }
122 
124  float pressure = (float) sensors.baro.value;
125  AbiSendMsgBARO_ABS(BARO_SIM_SENDER_ID, pressure);
126  Fbw(event_task);
127  Ap(event_task);
128  }
129 
131  AbiSendMsgTEMPERATURE(BARO_SIM_SENDER_ID, (float)sensors.temp.value);
132  }
133 
134 #if USE_AIRSPEED
137  Fbw(event_task);
138  Ap(event_task);
139  }
140 #endif
141 
143  gps_feed_value();
144  Fbw(event_task);
145  Ap(event_task);
146  }
147 
148  if (nps_bypass_ahrs) {
150  }
151 
152  if (nps_bypass_ins) {
154  }
155 
158 
159  /* scale final motor commands to 0-1 for feeding the fdm */
160 #ifdef NPS_ACTUATOR_NAMES
161  PRINT_CONFIG_MSG("actuators for JSBSim explicitly set.")
162  PRINT_CONFIG_VAR(NPS_COMMANDS_NB)
163  //PRINT_CONFIG_VAR(NPS_ACTUATOR_NAMES)
164 
165  for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) {
166  autopilot.commands[i] = (double)commands[i] / MAX_PPRZ;
167  }
168  // hack: invert pitch to fit most JSBSim models
169  autopilot.commands[COMMAND_PITCH] = -(double)commands[COMMAND_PITCH] / MAX_PPRZ;
170 #else
171  PRINT_CONFIG_MSG("Using throttle, roll, pitch, yaw commands instead of explicit actuators.")
172  PRINT_CONFIG_VAR(COMMAND_THROTTLE)
173  PRINT_CONFIG_VAR(COMMAND_ROLL)
174  PRINT_CONFIG_VAR(COMMAND_PITCH)
175 
176  autopilot.commands[COMMAND_THROTTLE] = (double)commands[COMMAND_THROTTLE] / MAX_PPRZ;
177  autopilot.commands[COMMAND_ROLL] = (double)commands[COMMAND_ROLL] / MAX_PPRZ;
178  // hack: invert pitch to fit most JSBSim models
179  autopilot.commands[COMMAND_PITCH] = -(double)commands[COMMAND_PITCH] / MAX_PPRZ;
180 #ifdef COMMAND_YAW
181  PRINT_CONFIG_VAR(COMMAND_YAW)
182  autopilot.commands[COMMAND_YAW] = (double)commands[COMMAND_YAW] / MAX_PPRZ;
183 #else
184  autopilot.commands[3] = 0.;
185 #endif
186 #endif
187 
188  // do the launch when clicking launch in GCS
190  if (!launch) {
191  autopilot.commands[COMMAND_THROTTLE] = 0;
192  }
193 }
194 
196 {
197 
198  struct FloatQuat quat_f;
199  QUAT_COPY(quat_f, fdm.ltp_to_body_quat);
200  stateSetNedToBodyQuat_f(&quat_f);
201 
202  struct FloatRates rates_f;
203  RATES_COPY(rates_f, fdm.body_ecef_rotvel);
204  stateSetBodyRates_f(&rates_f);
205 
206 }
207 
209 {
210 
212  struct NedCoor_f ltp_pos;
213  VECT3_COPY(ltp_pos, fdm.ltpprz_pos);
214  stateSetPositionNed_f(&ltp_pos);
215  }
216  else if (state.utm_initialized_f) {
217  struct LlaCoor_f lla;
218  LLA_COPY(lla, fdm.lla_pos);
219  struct UtmCoor_f utm;
220  utm.zone = (lla.lon / 1e7 + 180) / 6 + 1;
221  utm_of_lla_f(&utm, &lla);
222  stateSetPositionUtm_f(&utm);
223  }
224 
225  struct NedCoor_f ltp_speed;
226  VECT3_COPY(ltp_speed, fdm.ltpprz_ecef_vel);
227  stateSetSpeedNed_f(&ltp_speed);
228 
229  struct NedCoor_f ltp_accel;
230  VECT3_COPY(ltp_accel, fdm.ltpprz_ecef_accel);
231  stateSetAccelNed_f(&ltp_accel);
232 
233 }
bool utm_initialized_f
True if utm origin (float) coordinate frame is initialsed.
Definition: state.h:236
#define NPS_BYPASS_AHRS
struct NedCoor_d ltpprz_ecef_accel
accel in ltppprz frame, wrt ECEF frame
Definition: nps_fdm.h:81
bool nps_sensors_gps_available(void)
Definition: nps_sensors.c:67
void nps_autopilot_run_systime_step(void)
struct DoubleRates body_ecef_rotvel
Definition: nps_fdm.h:97
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
void nps_electrical_run_step(double time)
bool nps_sensors_mag_available(void)
Definition: nps_sensors.c:49
#define NPS_COMMANDS_NB
Number of commands sent to the FDM of NPS.
Definition: nps_autopilot.h:22
#define NPS_BYPASS_INS
AP ( AutoPilot ) process API.
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:139
bool ned_initialized_f
True if local float coordinate frame is initialsed.
Definition: state.h:223
bool nps_sensors_baro_available(void)
Definition: nps_sensors.c:58
Main include for ABI (AirBorneInterface).
double value
airspeed reading in meters/second
struct NpsSensorBaro baro
Definition: nps_sensors.h:19
bool launch
Definition: sim_ap.c:38
static void stateSetNedToBodyQuat_f(struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition: state.h:1075
NpsRadioControlType
position in UTM coordinates Units: meters
STATIC_INLINE void handle_periodic_tasks(void)
Definition: main.c:243
#define FALSE
Definition: std.h:5
struct NedCoor_d ltpprz_pos
Definition: nps_fdm.h:54
Roation quaternion.
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:532
bool kill_throttle
Definition: autopilot.c:42
static void stateSetPositionUtm_f(struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
Definition: state.h:576
vector in Latitude, Longitude and Altitude
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:803
void imu_feed_mag(void)
Definition: imu_nps.c:53
static void sys_tick_handler(void)
double value
temperature in degrees Celcius
vector in North East Down coordinates Units: meters
Architecture independent timing functions.
#define Fbw(f)
void nps_electrical_init(void)
Device independent GPS code (interface)
void nps_radio_control_init(enum NpsRadioControlType type, int num_script, char *js_dev)
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
void sim_overwrite_ahrs(void)
Hardware independent code for commands handling.
struct NpsSensors sensors
Definition: nps_sensors.c:6
Inertial Measurement Unit interface.
uint8_t zone
UTM zone number.
static void stateSetPositionNed_f(struct NedCoor_f *ned_pos)
Set position from local NED coordinates (float).
Definition: state.h:592
#define LLA_COPY(_pos1, _pos2)
Definition: pprz_geodetic.h:53
bool ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:171
void gps_feed_value(void)
Definition: gps_sim_nps.c:31
void sim_overwrite_ins(void)
struct LlaCoor_d lla_pos
Definition: nps_fdm.h:55
void imu_feed_gyro_accel(void)
Definition: imu_nps.c:40
void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char *rc_dev)
bool nps_sensors_temperature_available(void)
Definition: nps_sensors.c:94
#define BARO_SIM_SENDER_ID
unsigned char uint8_t
Definition: types.h:14
pprz_t commands[COMMANDS_NB]
Storage of intermediate command values.
Definition: commands.c:30
bool nps_bypass_ins
API to get/set the generic vehicle states.
bool nps_sensors_airspeed_available(void)
Definition: nps_sensors.c:85
bool nps_radio_control_available(double time)
Electrical status (bat voltage) for NPS.
void nps_autopilot_run_step(double time)
bool nps_bypass_ahrs
struct NpsAutopilot autopilot
static void stateSetAirspeed_f(float airspeed)
Set airspeed (float).
Definition: state.h:1291
bool nps_sensors_gyro_available(void)
Definition: nps_sensors.c:40
double value
pressure in Pascal
float lon
in radians
struct NpsSensorAirspeed airspeed
Definition: nps_sensors.h:22
struct NedCoor_d ltpprz_ecef_vel
velocity in ltppprz frame, wrt ECEF frame
Definition: nps_fdm.h:79
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:336
#define MAX_PPRZ
Definition: paparazzi.h:8
#define Ap(f)
FBW ( FlyByWire ) process API.
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1163
static void stateSetAccelNed_f(struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition: state.h:984
struct DoubleQuat ltp_to_body_quat
Definition: nps_fdm.h:91
double commands[NPS_COMMANDS_NB]
Definition: nps_autopilot.h:28
angular rates
struct NpsSensorTemperature temp
Definition: nps_sensors.h:23
struct State state
Definition: state.c:36
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
Fixedwing autopilot modes.