Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
nps_autopilot_rotorcraft.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
3 *
4 * This file is part of paparazzi.
5 *
6 * paparazzi is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2, or (at your option)
9 * any later version.
10 *
11 * paparazzi is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with paparazzi; see the file COPYING. If not, write to
18 * the Free Software Foundation, 59 Temple Place - Suite 330,
19 * Boston, MA 02111-1307, USA.
20 */
21
22#include "nps_autopilot.h"
23
24#include "main_ap.h"
25#include "nps_sensors.h"
26#include "nps_radio_control.h"
27#include "nps_electrical.h"
28#include "nps_fdm.h"
29
30#include "generated/modules.h"
32#include "modules/imu/imu.h"
33#include "mcu_periph/sys_time.h"
34#include "state.h"
35#include "modules/ahrs/ahrs.h"
36#include "modules/ins/ins.h"
37#include "math/pprz_algebra.h"
38
39#ifdef NPS_USE_COMMANDS
41#define NPS_NO_MOTOR_MIXING TRUE
42#endif
43
44#ifndef NPS_NO_MOTOR_MIXING
46#if NPS_COMMANDS_NB != MOTOR_MIXING_NB_MOTOR
47#warning "NPS_COMMANDS_NB does not match MOTOR_MIXING_NB_MOTOR!"
48#endif
49#endif
50
51#include "modules/core/abi.h"
52
53#include "pprzlink/messages.h"
55
56// for datalink_time hack
59
63
64#ifndef NPS_BYPASS_AHRS
65#define NPS_BYPASS_AHRS FALSE
66#endif
67
68#ifndef NPS_BYPASS_INS
69#define NPS_BYPASS_INS FALSE
70#endif
71
72#if INDI_RPM_FEEDBACK
73#error "INDI_RPM_FEEDBACK can not be used in simulation!"
74#endif
75
76void sys_tick_handler(void);
77
78void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char *rc_dev)
79{
80 nps_autopilot.launch = TRUE;
81
82 nps_radio_control_init(type_rc, num_rc_script, rc_dev);
84
87
88 modules_mcu_init();
90}
91
96
97#include <stdio.h>
98#include "modules/gps/gps.h"
99
100void nps_autopilot_run_step(double time)
101{
102
104
105#if RADIO_CONTROL && !RADIO_CONTROL_TYPE_DATALINK
106 if (nps_radio_control_available(time)) {
109 }
110#endif
111
115 }
116
118 imu_feed_mag();
120 }
121
123 uint32_t now_ts = get_sys_time_usec();
124 float pressure = (float) sensors.baro.value;
125 AbiSendMsgBARO_ABS_RAW(BARO_SIM_SENDER_ID, now_ts, pressure);
127 }
128
130 uint32_t now_ts = get_sys_time_usec();
131 AbiSendMsgTEMPERATURE_RAW(BARO_SIM_SENDER_ID, now_ts, (float)sensors.temp.value, TEMPERATURE_TYPE_AIR);
133 }
134
135#if USE_AIRSPEED
137 uint32_t now_ts = get_sys_time_usec();
138 AbiSendMsgAIRSPEED_RAW(AIRSPEED_NPS_ID, now_ts, (float)sensors.airspeed.value);
140 }
141#endif
142
143#if USE_SONAR
145 uint32_t now_ts = get_sys_time_usec();
146 float dist = (float) sensors.sonar.value;
147 if (dist >= 0.0) {
148 AbiSendMsgAGL(AGL_SONAR_NPS_ID, now_ts, dist);
149 }
150
151#ifdef SENSOR_SYNC_SEND_SONAR
152 uint16_t foo = 0;
153 DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &foo, &dist);
154#endif
155
157 }
158#endif
159
160#if USE_GPS
164 }
165#endif
166
167 if (nps_bypass_ahrs) {
169 }
170
171 if (nps_bypass_ins) {
173 }
174
176
177 /* scale final motor commands to 0-1 for feeding the fdm */
178 for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) {
179#if NPS_NO_MOTOR_MIXING
180 #if NPS_USE_COMMANDS
182 nps_autopilot.commands[i] = (double)commands[i] / MAX_PPRZ;
183 #else
185 nps_autopilot.commands[i] = (double)actuators_pprz[i] / MAX_PPRZ;
186 #endif
187#else
188 nps_autopilot.commands[i] = (double)motor_mixing.commands[i] / MAX_PPRZ;
189#endif
190 }
191}
192
193
195{
198
199 struct FloatQuat quat_f;
200 QUAT_COPY(quat_f, fdm.ltp_to_body_quat);
201 stateSetNedToBodyQuat_f(MODULE_NPS_ID, &quat_f);
202
203 struct FloatRates rates_f;
204 RATES_COPY(rates_f, fdm.body_ecef_rotvel);
205 stateSetBodyRates_f(MODULE_NPS_ID, &rates_f);
206
207}
208
210{
211
212 struct NedCoor_f ltp_pos;
213 VECT3_COPY(ltp_pos, fdm.ltpprz_pos);
214 stateSetPositionNed_f(MODULE_NPS_ID, &ltp_pos);
215
216 struct NedCoor_f ltp_speed;
217 VECT3_COPY(ltp_speed, fdm.ltpprz_ecef_vel);
218 stateSetSpeedNed_f(MODULE_NPS_ID, &ltp_speed);
219
220 struct NedCoor_f ltp_accel;
221 VECT3_COPY(ltp_accel, fdm.ltpprz_ecef_accel);
222 stateSetAccelNed_f(MODULE_NPS_ID, &ltp_accel);
223
224}
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 AGL_SONAR_NPS_ID
#define AIRSPEED_NPS_ID
bool autopilot_get_motors_on(void)
get motors status
Definition autopilot.c:303
#define sensors(...)
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Hardware independent code for commands handling.
Device independent GPS code (interface).
void gps_feed_value(void)
Definition gps_sim_nps.c:58
#define STATE_INPUT_RATES
Definition state.h:145
#define STATE_INPUT_ATTITUDE
Definition state.h:144
Roation quaternion.
angular rates
#define RATES_COPY(_a, _b)
#define QUAT_COPY(_qo, _qi)
#define VECT3_COPY(_a, _b)
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
void stateSetInputFilter(uint8_t type, uint16_t flag)
set the input filter for a specified type of data.
Definition state.c:91
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
Integrated Navigation System interface.
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
Hardware independent API for actuators (servos, motor controllers).
struct MotorMixing motor_mixing
Motor Mixing.
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
#define NPS_BYPASS_INS
#define NPS_BYPASS_AHRS
void nps_autopilot_run_step(double time)
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)
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_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
#define MAX_PPRZ
Definition paparazzi.h:8
Paparazzi generic algebra macros.
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
int16_t actuators_pprz[INDI_NUM_ACT+1]
PPRZ command to each actuator Can be used to directly control actuators from the control algorithm if...
API to get/set the generic vehicle states.
#define TRUE
Definition std.h:4
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.