Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 #include "main_ap.h"
26 
27 #include "nps_sensors.h"
28 #include "nps_radio_control.h"
29 #include "nps_electrical.h"
30 #include "nps_fdm.h"
31 
32 #include "generated/modules.h"
34 #include "modules/imu/imu.h"
35 #include "mcu_periph/sys_time.h"
36 #include "state.h"
37 #include "modules/core/commands.h"
38 
39 #include "modules/core/abi.h"
40 
41 // for launch
42 #include "autopilot.h"
43 
44 // for datalink_time hack
46 
47 #if USE_SONAR
48 // for sonar/lidar agl
50 #endif
51 
55 
56 #ifndef NPS_BYPASS_AHRS
57 #define NPS_BYPASS_AHRS FALSE
58 #endif
59 
60 #ifndef NPS_BYPASS_INS
61 #define NPS_BYPASS_INS FALSE
62 #endif
63 
64 
65 //#if !defined (FBW) || !defined (AP)
66 //#error NPS does not currently support dual processor simulation for FBW and AP on fixedwing!
67 //#endif
68 
69 void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char *rc_dev)
70 {
71 
73 
74  if (rc_dev != NULL) {
75  nps_radio_control_init(type_rc, num_rc_script, rc_dev);
76  }
78 
81 
82  modules_mcu_init();
83  main_ap_init();
84 
85 }
86 
88 {
90 }
91 
92 #include <stdio.h>
93 #include "modules/gps/gps.h"
94 
95 void nps_autopilot_run_step(double time)
96 {
97 
99 
100 #if RADIO_CONTROL && !RADIO_CONTROL_TYPE_DATALINK
101  if (nps_radio_control_available(time)) {
102  radio_control_feed();
103  main_ap_event();
104  }
105 #endif
106 
109  main_ap_event();
110  }
111 
113  imu_feed_mag();
114  main_ap_event();
115  }
116 
118  uint32_t now_ts = get_sys_time_usec();
119  float pressure = (float) sensors.baro.value;
120  AbiSendMsgBARO_ABS(BARO_SIM_SENDER_ID, now_ts, pressure);
121  main_ap_event();
122  }
123 
125  AbiSendMsgTEMPERATURE(BARO_SIM_SENDER_ID, (float)sensors.temp.value);
126  }
127 
128 #if USE_AIRSPEED || USE_NPS_AIRSPEED
130  AbiSendMsgAIRSPEED(AIRSPEED_NPS_ID, (float)sensors.airspeed.value);
131  main_ap_event();
132  }
133 #endif
134 
136  gps_feed_value();
137  main_ap_event();
138  }
139 
140 #if USE_SONAR
142  uint32_t now_ts = get_sys_time_usec();
143  float dist = (float) sensors.sonar.value;
144  AbiSendMsgAGL(AGL_SONAR_NPS_ID, now_ts, dist);
145 
146 #ifdef SENSOR_SYNC_SEND_SONAR
147  uint16_t foo = 0;
148  DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &foo, &dist);
149 #endif
150 
151  main_ap_event();
152  }
153 #endif
154 
155  // standalone angle of attack
156 #if USE_NPS_AOA && !NPS_SYNC_INCIDENCE
158  AbiSendMsgINCIDENCE(INCIDENCE_NPS_ID, 1, (float)sensors.aoa.value, 0.f);
159  main_ap_event();
160  }
161 #endif
162 
163  // standalone sideslip
164 #if USE_NPS_SIDESLIP && !NPS_SYNC_INCIDENCE
166  AbiSendMsgINCIDENCE(INCIDENCE_NPS_ID, 2, 0.f, (float)sensors.sideslip.value);
167  main_ap_event();
168  }
169 #endif
170 
171  // synchronized angle of attack and sideslip
172 #if NPS_SYNC_INCIDENCE && USE_NPS_AOA && USE_NPS_SIDESLIP
173  static uint8_t flag = 0;
175  SetBit(flag, 0);
176  }
178  SetBit(flag, 1);
179  }
180  if (flag == 3) {
181  // both sensors are updated
182  AbiSendMsgINCIDENCE(INCIDENCE_NPS_ID, 3, (float)sensors.aoa.value, (float)sensors.sideslip.value);
183  main_ap_event();
184  flag = 0;
185  }
186 #endif
187 
188 
189 
190  if (nps_bypass_ahrs) {
192  }
193 
194  if (nps_bypass_ins) {
196  }
197 
199 
200  /* scale final motor commands to 0-1 for feeding the fdm */
201 #ifdef NPS_ACTUATOR_NAMES
202  PRINT_CONFIG_MSG("actuators for JSBSim explicitly set.")
203  PRINT_CONFIG_VAR(NPS_COMMANDS_NB)
204  //PRINT_CONFIG_VAR(NPS_ACTUATOR_NAMES)
205 
206  for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) {
207  nps_autopilot.commands[i] = (double)commands[i] / MAX_PPRZ;
208  }
209  // hack: invert pitch to fit most JSBSim models
210  nps_autopilot.commands[COMMAND_PITCH] = -(double)commands[COMMAND_PITCH] / MAX_PPRZ;
211 #else
212  PRINT_CONFIG_MSG("Using throttle, roll, pitch, yaw commands instead of explicit actuators.")
213  PRINT_CONFIG_VAR(COMMAND_THROTTLE)
214  PRINT_CONFIG_VAR(COMMAND_ROLL)
215  PRINT_CONFIG_VAR(COMMAND_PITCH)
216 
217  nps_autopilot.commands[COMMAND_THROTTLE] = (double)commands[COMMAND_THROTTLE] / MAX_PPRZ;
218  nps_autopilot.commands[COMMAND_ROLL] = (double)commands[COMMAND_ROLL] / MAX_PPRZ;
219  // hack: invert pitch to fit most JSBSim models
220  nps_autopilot.commands[COMMAND_PITCH] = -(double)commands[COMMAND_PITCH] / MAX_PPRZ;
221 #ifdef COMMAND_YAW
222  PRINT_CONFIG_VAR(COMMAND_YAW)
223  nps_autopilot.commands[COMMAND_YAW] = (double)commands[COMMAND_YAW] / MAX_PPRZ;
224 #endif /* COMMAND_YAW */
225 #endif /* NPS_ACTUATOR_NAMES */
226 
227  // do the launch when clicking launch in GCS
229  if (!autopilot.launch) {
230  nps_autopilot.commands[COMMAND_THROTTLE] = 0;
231  }
232 }
233 
235 {
236 
237  struct FloatQuat quat_f;
238  QUAT_COPY(quat_f, fdm.ltp_to_body_quat);
239  stateSetNedToBodyQuat_f(&quat_f);
240 
241  struct FloatRates rates_f;
242  RATES_COPY(rates_f, fdm.body_ecef_rotvel);
243  stateSetBodyRates_f(&rates_f);
244 
245 }
246 
248 {
249 
251  struct NedCoor_f ltp_pos;
252  VECT3_COPY(ltp_pos, fdm.ltpprz_pos);
253  stateSetPositionNed_f(&ltp_pos);
254  }
255  else if (state.utm_initialized_f) {
256  struct LlaCoor_f lla;
257  LLA_COPY(lla, fdm.lla_pos);
258  struct UtmCoor_f utm;
259  utm.zone = (lla.lon / 1e7 + 180) / 6 + 1;
260  utm_of_lla_f(&utm, &lla);
261  stateSetPositionUtm_f(&utm);
262  }
263 
264  struct NedCoor_f ltp_speed;
265  VECT3_COPY(ltp_speed, fdm.ltpprz_ecef_vel);
266  stateSetSpeedNed_f(&ltp_speed);
267 
268  struct NedCoor_f ltp_accel;
269  VECT3_COPY(ltp_accel, fdm.ltpprz_ecef_accel);
270  stateSetAccelNed_f(&ltp_accel);
271 
272 }
NPS_BYPASS_INS
#define NPS_BYPASS_INS
Definition: nps_autopilot_fixedwing.c:61
radio_control.h
main_ap_init
void main_ap_init(void)
Definition: main_ap.c:76
NPS_COMMANDS_NB
#define NPS_COMMANDS_NB
Number of commands sent to the FDM of NPS.
Definition: nps_autopilot.h:42
MAX_PPRZ
#define MAX_PPRZ
Definition: paparazzi.h:8
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
nps_sensors_aoa_available
bool nps_sensors_aoa_available(void)
Definition: nps_sensors.c:106
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
NedCoor_f
vector in North East Down coordinates Units: meters
Definition: pprz_geodetic_float.h:63
AIRSPEED_NPS_ID
#define AIRSPEED_NPS_ID
Definition: abi_sender_ids.h:99
nps_sensors_sonar_available
bool nps_sensors_sonar_available(void)
Definition: nps_sensors.c:79
stateSetSpeedNed_f
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:809
NPS_BYPASS_AHRS
#define NPS_BYPASS_AHRS
Definition: nps_autopilot_fixedwing.c:57
nps_sensors_sideslip_available
bool nps_sensors_sideslip_available(void)
Definition: nps_sensors.c:115
LLA_COPY
#define LLA_COPY(_pos1, _pos2)
Definition: pprz_geodetic.h:58
abi.h
nps_sensors_gyro_available
bool nps_sensors_gyro_available(void)
Definition: nps_sensors.c:43
NpsFdm::ltpprz_ecef_vel
struct NedCoor_d ltpprz_ecef_vel
velocity in ltppprz frame, wrt ECEF frame
Definition: nps_fdm.h:79
NpsRadioControlType
NpsRadioControlType
Definition: nps_radio_control.h:31
nps_autopilot_run_systime_step
void nps_autopilot_run_systime_step(void)
Definition: nps_autopilot_fixedwing.c:87
LlaCoor_f::lon
float lon
in radians
Definition: pprz_geodetic_float.h:56
nps_sensors_temperature_available
bool nps_sensors_temperature_available(void)
Definition: nps_sensors.c:97
commands
pprz_t commands[COMMANDS_NB]
Definition: commands.c:30
NpsFdm::ltpprz_ecef_accel
struct NedCoor_d ltpprz_ecef_accel
accel in ltppprz frame, wrt ECEF frame
Definition: nps_fdm.h:81
nps_fdm.h
stateSetPositionUtm_f
static void stateSetPositionUtm_f(struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
Definition: state.h:582
NpsFdm::ltp_to_body_quat
struct DoubleQuat ltp_to_body_quat
Definition: nps_fdm.h:91
nps_sensors_mag_available
bool nps_sensors_mag_available(void)
Definition: nps_sensors.c:52
nps_autopilot.h
imu_feed_gyro_accel
void imu_feed_gyro_accel(void)
Definition: imu_nps.c:40
nps_sensors_baro_available
bool nps_sensors_baro_available(void)
Definition: nps_sensors.c:61
imu_feed_mag
void imu_feed_mag(void)
Definition: imu_nps.c:53
foo
uint16_t foo
Definition: main_demo5.c:58
State::ned_initialized_i
bool ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:171
main_ap_periodic
void main_ap_periodic(void)
Definition: main_ap.c:121
utm_of_lla_f
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
Definition: pprz_geodetic_float.c:308
nps_sensors_airspeed_available
bool nps_sensors_airspeed_available(void)
Definition: nps_sensors.c:88
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:75
fdm
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
Definition: nps_fdm_crrcsim.c:84
AGL_SONAR_NPS_ID
#define AGL_SONAR_NPS_ID
Definition: abi_sender_ids.h:145
imu.h
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
sim_overwrite_ahrs
void sim_overwrite_ahrs(void)
Definition: nps_autopilot_fixedwing.c:234
NpsFdm::body_ecef_rotvel
struct DoubleRates body_ecef_rotvel
Definition: nps_fdm.h:97
main_ap.h
nps_radio_control.h
BARO_SIM_SENDER_ID
#define BARO_SIM_SENDER_ID
Definition: abi_sender_ids.h:72
gps.h
Device independent GPS code (interface)
autopilot
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:48
UtmCoor_f::zone
uint8_t zone
UTM zone number.
Definition: pprz_geodetic_float.h:85
gps_feed_value
void gps_feed_value(void)
Definition: gps_sim_nps.c:31
stateSetBodyRates_f
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
NpsAutopilot::launch
bool launch
Definition: nps_autopilot.h:48
State::utm_initialized_f
bool utm_initialized_f
True if utm origin (float) coordinate frame is initialsed.
Definition: state.h:236
nps_sensors_gps_available
bool nps_sensors_gps_available(void)
Definition: nps_sensors.c:70
sys_time.h
Architecture independent timing functions.
nps_radio_control_init
void nps_radio_control_init(enum NpsRadioControlType type, int num_script, char *js_dev)
Definition: nps_radio_control.c:35
nps_electrical_run_step
void nps_electrical_run_step(double time)
Definition: nps_electrical.c:44
nps_autopilot
struct NpsAutopilot nps_autopilot
Definition: nps_autopilot_fixedwing.c:52
nps_sensors.h
autopilot.h
sim_overwrite_ins
void sim_overwrite_ins(void)
Definition: nps_autopilot_fixedwing.c:247
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
stateSetAccelNed_f
static void stateSetAccelNed_f(struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition: state.h:1002
PRINT_CONFIG_MSG
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
NpsFdm::lla_pos
struct LlaCoor_d lla_pos
Definition: nps_fdm.h:55
QUAT_COPY
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:596
nps_radio_control_available
bool nps_radio_control_available(double time)
Definition: nps_radio_control.c:81
nps_electrical_init
void nps_electrical_init(void)
Definition: nps_electrical.c:33
NpsFdm::ltpprz_pos
struct NedCoor_d ltpprz_pos
Definition: nps_fdm.h:54
stateSetPositionNed_f
static void stateSetPositionNed_f(struct NedCoor_f *ned_pos)
Set position from local NED coordinates (float).
Definition: state.h:598
UtmCoor_f
position in UTM coordinates Units: meters
Definition: pprz_geodetic_float.h:81
nps_electrical.h
nps_autopilot_init
void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char *rc_dev)
Definition: nps_autopilot_fixedwing.c:69
State::ned_initialized_f
bool ned_initialized_f
True if local float coordinate frame is initialsed.
Definition: state.h:223
commands.h
Hardware independent code for commands handling.
nps_bypass_ins
bool nps_bypass_ins
Definition: nps_autopilot_fixedwing.c:54
state.h
FALSE
#define FALSE
Definition: std.h:5
NpsAutopilot::commands
double commands[NPS_COMMANDS_NB]
Definition: nps_autopilot.h:47
RATES_COPY
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:337
NpsAutopilot
Definition: nps_autopilot.h:46
nps_autopilot_run_step
void nps_autopilot_run_step(double time)
Definition: nps_autopilot_fixedwing.c:95
uint16_t
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
state
struct State state
Definition: state.c:36
nps_bypass_ahrs
bool nps_bypass_ahrs
Definition: nps_autopilot_fixedwing.c:53
sys_tick_handler
static void sys_tick_handler(void)
Definition: sys_time_arch.c:142
pprz_autopilot::kill_throttle
bool kill_throttle
allow autopilot to use throttle
Definition: autopilot.h:69
INCIDENCE_NPS_ID
#define INCIDENCE_NPS_ID
Definition: abi_sender_ids.h:130
pprz_autopilot::launch
bool launch
request launch
Definition: autopilot.h:71
sensors
#define sensors(...)
Definition: cc2500_compat.h:68
VECT3_COPY
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
stateSetNedToBodyQuat_f
static void stateSetNedToBodyQuat_f(struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition: state.h:1093
FloatRates
angular rates
Definition: pprz_algebra_float.h:93
main_ap_event
void main_ap_event(void)
Definition: main_ap.c:148
LlaCoor_f
vector in Latitude, Longitude and Altitude
Definition: pprz_geodetic_float.h:54