Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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"
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
64void sys_tick_handler(void);
65
66//#if !defined (FBW) || !defined (AP)
67//#error NPS does not currently support dual processor simulation for FBW and AP on fixedwing!
68//#endif
69
70void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char *rc_dev)
71{
72
73 nps_autopilot.launch = FALSE;
74
75 if (rc_dev != NULL) {
76 nps_radio_control_init(type_rc, num_rc_script, rc_dev);
77 }
79
82
83 modules_mcu_init();
85
86}
87
92
93#include <stdio.h>
94#include "modules/gps/gps.h"
95
96void nps_autopilot_run_step(double time)
97{
98
100
101#if RADIO_CONTROL && !RADIO_CONTROL_TYPE_DATALINK
102 if (nps_radio_control_available(time)) {
105 }
106#endif
107
111 }
112
114 imu_feed_mag();
116 }
117
119 uint32_t now_ts = get_sys_time_usec();
120 float pressure = (float) sensors.baro.value;
121 AbiSendMsgBARO_ABS_RAW(BARO_SIM_SENDER_ID, now_ts, pressure);
123 }
124
126 uint32_t now_ts = get_sys_time_usec();
127 AbiSendMsgTEMPERATURE_RAW(BARO_SIM_SENDER_ID, now_ts, (float)sensors.temp.value, TEMPERATURE_TYPE_AIR);
128 }
129
130#if USE_AIRSPEED || USE_NPS_AIRSPEED
132 uint32_t now_ts = get_sys_time_usec();
133 AbiSendMsgAIRSPEED_RAW(AIRSPEED_NPS_ID, now_ts, (float)sensors.airspeed.value);
135 }
136#endif
137
141 }
142
143#if USE_SONAR
145 uint32_t now_ts = get_sys_time_usec();
146 float dist = (float) sensors.sonar.value;
147 AbiSendMsgAGL(AGL_SONAR_NPS_ID, now_ts, dist);
148
149#ifdef SENSOR_SYNC_SEND_SONAR
150 uint16_t foo = 0;
151 DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &foo, &dist);
152#endif
153
155 }
156#endif
157
158 // standalone angle of attack
159#if USE_NPS_AOA && !NPS_SYNC_INCIDENCE
161 AbiSendMsgINCIDENCE(INCIDENCE_NPS_ID, 1, (float)sensors.aoa.value, 0.f);
163 }
164#endif
165
166 // standalone sideslip
167#if USE_NPS_SIDESLIP && !NPS_SYNC_INCIDENCE
169 AbiSendMsgINCIDENCE(INCIDENCE_NPS_ID, 2, 0.f, (float)sensors.sideslip.value);
171 }
172#endif
173
174 // synchronized angle of attack and sideslip
175#if NPS_SYNC_INCIDENCE && USE_NPS_AOA && USE_NPS_SIDESLIP
176 static uint8_t flag = 0;
178 SetBit(flag, 0);
179 }
181 SetBit(flag, 1);
182 }
183 if (flag == 3) {
184 // both sensors are updated
185 AbiSendMsgINCIDENCE(INCIDENCE_NPS_ID, 3, (float)sensors.aoa.value, (float)sensors.sideslip.value);
187 flag = 0;
188 }
189#endif
190
191
192
193 if (nps_bypass_ahrs) {
195 }
196
197 if (nps_bypass_ins) {
199 }
200
202
203 /* scale final motor commands to 0-1 for feeding the fdm */
204#ifdef NPS_ACTUATOR_NAMES
205 PRINT_CONFIG_MSG("actuators for JSBSim explicitly set.")
207 //PRINT_CONFIG_VAR(NPS_ACTUATOR_NAMES)
208
209 for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) {
210 nps_autopilot.commands[i] = (double)commands[i] / MAX_PPRZ;
211 }
212 // hack: invert pitch to fit most JSBSim models
214#else
215 PRINT_CONFIG_MSG("Using throttle, roll, pitch, yaw commands instead of explicit actuators.")
216 PRINT_CONFIG_VAR(COMMAND_THROTTLE)
219
220 nps_autopilot.commands[COMMAND_THROTTLE] = (double)commands[COMMAND_THROTTLE] / MAX_PPRZ;
222 // hack: invert pitch to fit most JSBSim models
224#ifdef COMMAND_YAW
225 PRINT_CONFIG_VAR(COMMAND_YAW)
226 nps_autopilot.commands[COMMAND_YAW] = (double)commands[COMMAND_YAW] / MAX_PPRZ;
227#endif /* COMMAND_YAW */
228#endif /* NPS_ACTUATOR_NAMES */
229
230 // do the launch when clicking launch in GCS
231 nps_autopilot.launch = autopilot.launch && !autopilot.kill_throttle;
232 if (!autopilot.launch) {
233 nps_autopilot.commands[COMMAND_THROTTLE] = 0;
234 }
235}
236
238{
239
240 struct FloatQuat quat_f;
241 QUAT_COPY(quat_f, fdm.ltp_to_body_quat);
242 stateSetNedToBodyQuat_f(MODULE_NPS_ID, &quat_f);
243
244 struct FloatRates rates_f;
245 RATES_COPY(rates_f, fdm.body_ecef_rotvel);
246 stateSetBodyRates_f(MODULE_NPS_ID, &rates_f);
247
248}
249
251{
252
253 if (state.ned_initialized_i || state.ned_initialized_f) {
254 struct NedCoor_f ltp_pos;
255 VECT3_COPY(ltp_pos, fdm.ltpprz_pos);
256 stateSetPositionNed_f(MODULE_NPS_ID, &ltp_pos);
257 }
258 else if (state.utm_initialized_f) {
259 struct LlaCoor_f lla;
260 LLA_COPY(lla, fdm.lla_pos);
261 stateSetPositionLla_f(MODULE_NPS_ID, &lla);
262 }
263
264 struct NedCoor_f ltp_speed;
265 VECT3_COPY(ltp_speed, fdm.ltpprz_ecef_vel);
266 stateSetSpeedNed_f(MODULE_NPS_ID, &ltp_speed);
267
268 struct NedCoor_f ltp_accel;
269 VECT3_COPY(ltp_accel, fdm.ltpprz_ecef_accel);
270 stateSetAccelNed_f(MODULE_NPS_ID, &ltp_accel);
271
272}
Main include for ABI (AirBorneInterface).
#define TEMPERATURE_TYPE_AIR
Types of the TEMPERATURE_RAW message, what the temperature measures.
Definition abi_common.h:89
#define BARO_SIM_SENDER_ID
#define INCIDENCE_NPS_ID
#define AGL_SONAR_NPS_ID
#define AIRSPEED_NPS_ID
struct pprz_autopilot autopilot
Global autopilot structure.
Definition autopilot.c:50
Core autopilot interface common to all firmwares.
#define sensors(...)
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Hardware independent code for commands handling.
#define COMMAND_PITCH
#define COMMAND_ROLL
Device independent GPS code (interface).
void gps_feed_value(void)
Definition gps_sim_nps.c:58
Roation quaternion.
angular rates
#define RATES_COPY(_a, _b)
#define QUAT_COPY(_qo, _qi)
#define VECT3_COPY(_a, _b)
#define LLA_COPY(_pos1, _pos2)
static void stateSetAccelNed_f(uint16_t id, struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition state.h:1166
static void stateSetNedToBodyQuat_f(uint16_t id, struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition state.h:1272
struct State state
Definition state.c:36
static void stateSetPositionLla_f(uint16_t id, struct LlaCoor_f *lla_pos)
Set position from LLA coordinates (float).
Definition state.h:749
static void stateSetPositionNed_f(uint16_t id, struct NedCoor_f *ned_pos)
Set position from local NED coordinates (float).
Definition state.h:729
static void stateSetBodyRates_f(uint16_t id, struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition state.h:1371
static void stateSetSpeedNed_f(uint16_t id, struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition state.h:958
Inertial Measurement Unit interface.
void imu_feed_mag(void)
Definition imu_nps.c:61
void imu_feed_gyro_accel(void)
Definition imu_nps.c:50
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
void main_ap_periodic(void)
Definition main_ap.c:119
void main_ap_event(void)
Definition main_ap.c:146
void main_ap_init(void)
Definition main_ap.c:79
Autopilot main loop.
uint16_t foo
Definition main_demo5.c:58
struct NpsAutopilot nps_autopilot
ifndef NPS_NO_MOTOR_MIXING include "modules/actuators/motor_mixing.h"
bool nps_bypass_ahrs
#define NPS_COMMANDS_NB
Number of commands sent to the FDM of NPS.
bool nps_bypass_ins
void nps_autopilot_run_step(double time)
#define NPS_BYPASS_INS
void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char *rc_dev)
void sim_overwrite_ins(void)
void nps_autopilot_run_systime_step(void)
void sim_overwrite_ahrs(void)
#define NPS_BYPASS_AHRS
void sys_tick_handler(void)
void nps_electrical_init(void)
void nps_electrical_run_step(double time)
Electrical status (bat voltage) for NPS.
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
void nps_radio_control_init(enum NpsRadioControlType type, int num_script, char *js_dev)
bool nps_radio_control_available(double time)
NpsRadioControlType
bool nps_sensors_airspeed_available(void)
Definition nps_sensors.c:85
bool nps_sensors_sonar_available(void)
Definition nps_sensors.c:76
bool nps_sensors_mag_available(void)
Definition nps_sensors.c:49
bool nps_sensors_gps_available(void)
Definition nps_sensors.c:67
bool nps_sensors_sideslip_available(void)
bool nps_sensors_aoa_available(void)
bool nps_sensors_baro_available(void)
Definition nps_sensors.c:58
bool nps_sensors_gyro_available(void)
Definition nps_sensors.c:40
bool nps_sensors_temperature_available(void)
Definition nps_sensors.c:94
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
#define MAX_PPRZ
Definition paparazzi.h:8
vector in Latitude, Longitude and Altitude
vector in North East Down coordinates Units: meters
Generic interface for radio control modules.
static const ShellCommand commands[]
Definition shell_arch.c:77
void radio_control_feed(void)
Definition ppm_arch.c:42
API to get/set the generic vehicle states.
#define FALSE
Definition std.h:5
Architecture independent timing functions.
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.