Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 
31 #include "modules/imu/imu.h"
32 #include "mcu_periph/sys_time.h"
33 #include "state.h"
34 #include "modules/ahrs/ahrs.h"
35 #include "modules/ins/ins.h"
36 #include "math/pprz_algebra.h"
37 
38 #ifndef NPS_NO_MOTOR_MIXING
40 
41 #if NPS_COMMANDS_NB != MOTOR_MIXING_NB_MOTOR
42 #warning "NPS_COMMANDS_NB does not match MOTOR_MIXING_NB_MOTOR!"
43 #endif
44 #endif
45 
46 #include "modules/core/abi.h"
47 
48 #include "pprzlink/messages.h"
50 
51 // for datalink_time hack
54 
58 
59 #ifndef NPS_BYPASS_AHRS
60 #define NPS_BYPASS_AHRS FALSE
61 #endif
62 
63 #ifndef NPS_BYPASS_INS
64 #define NPS_BYPASS_INS FALSE
65 #endif
66 
67 #if INDI_RPM_FEEDBACK
68 #error "INDI_RPM_FEEDBACK can not be used in simulation!"
69 #endif
70 
71 void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char *rc_dev)
72 {
74 
75  nps_radio_control_init(type_rc, num_rc_script, rc_dev);
77 
80 
81  modules_mcu_init();
82  main_ap_init();
83 }
84 
86 {
88 }
89 
90 #include <stdio.h>
91 #include "modules/gps/gps.h"
92 
93 void nps_autopilot_run_step(double time)
94 {
95 
97 
98 #if RADIO_CONTROL && !RADIO_CONTROL_TYPE_DATALINK
99  if (nps_radio_control_available(time)) {
100  radio_control_feed();
101  main_ap_event();
102  }
103 #endif
104 
107  main_ap_event();
108  }
109 
111  imu_feed_mag();
112  main_ap_event();
113  }
114 
116  uint32_t now_ts = get_sys_time_usec();
117  float pressure = (float) sensors.baro.value;
118  AbiSendMsgBARO_ABS(BARO_SIM_SENDER_ID, now_ts, pressure);
119  main_ap_event();
120  }
121 
123  AbiSendMsgTEMPERATURE(BARO_SIM_SENDER_ID, (float)sensors.temp.value);
124  main_ap_event();
125  }
126 
127 #if USE_AIRSPEED
129  stateSetAirspeed_f((float)sensors.airspeed.value);
130  AbiSendMsgAIRSPEED(AIRSPEED_NPS_ID, (float)sensors.airspeed.value);
131  main_ap_event();
132  }
133 #endif
134 
135 #if USE_SONAR
137  uint32_t now_ts = get_sys_time_usec();
138  float dist = (float) sensors.sonar.value;
139  if (dist >= 0.0) {
140  AbiSendMsgAGL(AGL_SONAR_NPS_ID, now_ts, dist);
141  }
142 
143 #ifdef SENSOR_SYNC_SEND_SONAR
144  uint16_t foo = 0;
145  DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &foo, &dist);
146 #endif
147 
148  main_ap_event();
149  }
150 #endif
151 
152 #if USE_GPS
154  gps_feed_value();
155  main_ap_event();
156  }
157 #endif
158 
159  if (nps_bypass_ahrs) {
161  }
162 
163  if (nps_bypass_ins) {
165  }
166 
168 
169  /* scale final motor commands to 0-1 for feeding the fdm */
170  for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) {
171 #if NPS_NO_MOTOR_MIXING
172  actuators_pprz[i] = autopilot_get_motors_on() ? actuators_pprz[i] : 0;
173  nps_autopilot.commands[i] = (double)actuators_pprz[i] / MAX_PPRZ;
174 #else
176 #endif
177  }
178 }
179 
180 
182 {
183 
184  struct FloatQuat quat_f;
185  QUAT_COPY(quat_f, fdm.ltp_to_body_quat);
186  stateSetNedToBodyQuat_f(&quat_f);
187 
188  struct FloatRates rates_f;
189  RATES_COPY(rates_f, fdm.body_ecef_rotvel);
190  stateSetBodyRates_f(&rates_f);
191 
192 }
193 
195 {
196 
197  struct NedCoor_f ltp_pos;
198  VECT3_COPY(ltp_pos, fdm.ltpprz_pos);
199  stateSetPositionNed_f(&ltp_pos);
200 
201  struct NedCoor_f ltp_speed;
202  VECT3_COPY(ltp_speed, fdm.ltpprz_ecef_vel);
203  stateSetSpeedNed_f(&ltp_speed);
204 
205  struct NedCoor_f ltp_accel;
206  VECT3_COPY(ltp_accel, fdm.ltpprz_ecef_accel);
207  stateSetAccelNed_f(&ltp_accel);
208 
209 }
autopilot_get_motors_on
bool autopilot_get_motors_on(void)
get motors status
Definition: autopilot.c:244
radio_control.h
main_ap_init
void main_ap_init(void)
Definition: main_ap.c:76
stateSetAirspeed_f
static void stateSetAirspeed_f(float airspeed)
Set airspeed (float).
Definition: state.h:1309
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
ins.h
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
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:76
stateSetSpeedNed_f
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:809
motor_mixing.h
abi.h
NPS_BYPASS_INS
#define NPS_BYPASS_INS
Definition: nps_autopilot_rotorcraft.c:64
nps_sensors_gyro_available
bool nps_sensors_gyro_available(void)
Definition: nps_sensors.c:40
NpsFdm::ltpprz_ecef_vel
struct NedCoor_d ltpprz_ecef_vel
velocity in ltppprz frame, wrt ECEF frame
Definition: nps_fdm.h:79
sim_overwrite_ins
void sim_overwrite_ins(void)
Definition: nps_autopilot_rotorcraft.c:194
NpsRadioControlType
NpsRadioControlType
Definition: nps_radio_control.h:31
nps_sensors_temperature_available
bool nps_sensors_temperature_available(void)
Definition: nps_sensors.c:94
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
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:49
nps_autopilot_run_step
void nps_autopilot_run_step(double time)
Definition: nps_autopilot_rotorcraft.c:93
nps_autopilot.h
imu_feed_gyro_accel
void imu_feed_gyro_accel(void)
Definition: imu_nps.c:66
sim_overwrite_ahrs
void sim_overwrite_ahrs(void)
Definition: nps_autopilot_rotorcraft.c:181
nps_sensors_baro_available
bool nps_sensors_baro_available(void)
Definition: nps_sensors.c:58
imu_feed_mag
void imu_feed_mag(void)
Definition: imu_nps.c:79
foo
uint16_t foo
Definition: main_demo5.c:58
nps_autopilot_run_systime_step
void nps_autopilot_run_systime_step(void)
Definition: nps_autopilot_rotorcraft.c:85
main_ap_periodic
void main_ap_periodic(void)
Definition: main_ap.c:116
nps_sensors_airspeed_available
bool nps_sensors_airspeed_available(void)
Definition: nps_sensors.c:85
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
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)
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
nps_sensors_gps_available
bool nps_sensors_gps_available(void)
Definition: nps_sensors.c:67
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_bypass_ahrs
bool nps_bypass_ahrs
Definition: nps_autopilot_rotorcraft.c:56
nps_electrical_run_step
void nps_electrical_run_step(double time)
Definition: nps_electrical.c:44
nps_sensors.h
if
if(GpsFixValid() &&e_identification_started)
Definition: e_identification_fr.c:159
ahrs.h
stateSetAccelNed_f
static void stateSetAccelNed_f(struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition: state.h:1002
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
actuators.h
stateSetPositionNed_f
static void stateSetPositionNed_f(struct NedCoor_f *ned_pos)
Set position from local NED coordinates (float).
Definition: state.h:598
nps_electrical.h
pprz_algebra.h
Paparazzi generic algebra macros.
state.h
nps_autopilot_init
void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char *rc_dev)
Definition: nps_autopilot_rotorcraft.c:71
NpsAutopilot::commands
double commands[NPS_COMMANDS_NB]
Definition: nps_autopilot.h:47
RATES_COPY
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:337
TRUE
#define TRUE
Definition: std.h:4
NpsAutopilot
Definition: nps_autopilot.h:46
uint16_t
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
nps_autopilot
struct NpsAutopilot nps_autopilot
Definition: nps_autopilot_rotorcraft.c:55
sys_tick_handler
static void sys_tick_handler(void)
Definition: sys_time_arch.c:142
MotorMixing::commands
int32_t commands[MOTOR_MIXING_NB_MOTOR]
Definition: motor_mixing.h:37
NPS_BYPASS_AHRS
#define NPS_BYPASS_AHRS
Definition: nps_autopilot_rotorcraft.c:60
sensors
#define sensors(...)
Definition: cc2500_compat.h:68
motor_mixing
struct MotorMixing motor_mixing
Definition: motor_mixing.c:94
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
nps_bypass_ins
bool nps_bypass_ins
Definition: nps_autopilot_rotorcraft.c:57
FloatRates
angular rates
Definition: pprz_algebra_float.h:93
main_ap_event
void main_ap_event(void)
Definition: main_ap.c:143