Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
nps_autopilot_rover.c
Go to the documentation of this file.
1/*
2 *
3 * This file is part of paparazzi.
4 *
5 * paparazzi is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2, or (at your option)
8 * any later version.
9 *
10 * paparazzi is distributed in the hope that it will be useful,
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 * GNU General Public License for more details.
14 *
15 * You should have received a copy of the GNU General Public License
16 * along with paparazzi; see the file COPYING. If not, write to
17 * the Free Software Foundation, 59 Temple Place - Suite 330,
18 * Boston, MA 02111-1307, USA.
19 */
20
21#include "nps_autopilot.h"
22
23#include "main_ap.h"
24#include "nps_sensors.h"
25#include "nps_radio_control.h"
26#include "nps_electrical.h"
27#include "nps_fdm.h"
28
30#include "modules/imu/imu.h"
31#include "mcu_periph/sys_time.h"
32#include "state.h"
33#include "modules/ahrs/ahrs.h"
34#include "modules/ins/ins.h"
35#include "math/pprz_algebra.h"
36
46
47#include "modules/core/abi.h"
48
49#include "pprzlink/messages.h"
51
52// for datalink_time hack
55
59
60#ifndef NPS_BYPASS_AHRS
61#define NPS_BYPASS_AHRS FALSE
62#endif
63
64#ifndef NPS_BYPASS_INS
65#define NPS_BYPASS_INS FALSE
66#endif
67
68#if INDI_RPM_FEEDBACK
69#error "INDI_RPM_FEEDBACK can not be used in simulation!"
70#endif
71
72void sys_tick_handler(void);
73
74void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char *rc_dev)
75{
76 nps_autopilot.launch = TRUE;
77
78 nps_radio_control_init(type_rc, num_rc_script, rc_dev);
80
83
84 modules_mcu_init();
86
87}
88
93
94#include <stdio.h>
95#include "modules/gps/gps.h"
96
97void nps_autopilot_run_step(double time)
98{
99
101
102#if RADIO_CONTROL && !RADIO_CONTROL_TYPE_DATALINK
103 if (nps_radio_control_available(time)) {
106 }
107#endif
108
112 }
113
115 imu_feed_mag();
117 }
118
120 uint32_t now_ts = get_sys_time_usec();
121 float pressure = (float) sensors.baro.value;
122 AbiSendMsgBARO_ABS_RAW(BARO_SIM_SENDER_ID, now_ts, pressure);
124 }
125
127 uint32_t now_ts = get_sys_time_usec();
128 AbiSendMsgTEMPERATURE_RAW(BARO_SIM_SENDER_ID, now_ts, (float)sensors.temp.value, TEMPERATURE_TYPE_AIR);
130 }
131
132#if USE_AIRSPEED
134 stateSetAirspeed_f(MODULE_NPS_ID, (float)sensors.airspeed.value);
135 uint32_t now_ts = get_sys_time_usec();
136 AbiSendMsgAIRSPEED_RAW(AIRSPEED_NPS_ID, now_ts, (float)sensors.airspeed.value);
138 }
139#endif
140
141#if USE_SONAR
143 uint32_t now_ts = get_sys_time_usec();
144 float dist = (float) sensors.sonar.value;
145 if (dist >= 0.0) {
146 AbiSendMsgAGL(AGL_SONAR_NPS_ID, now_ts, dist);
147 }
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#if USE_GPS
162 }
163#endif
164
165 if (nps_bypass_ahrs) {
167 }
168
169 if (nps_bypass_ins) {
171 }
172
174
175 /* feeding the fdm with raw low level commands */
176 for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) {
178 nps_autopilot.commands[i] = (double)commands[i] / MAX_PPRZ;
179 }
180
181 PRINT_CONFIG_MSG("Using throttle, steering commands because rover's fdm don't have explicit actuators.")
182}
183
184
186{
188
189 struct FloatQuat quat_f;
190 QUAT_COPY(quat_f, fdm.ltp_to_body_quat);
191 stateSetNedToBodyQuat_f(MODULE_NPS_ID, &quat_f);
192
193 struct FloatRates rates_f;
194 RATES_COPY(rates_f, fdm.body_ecef_rotvel);
195 stateSetBodyRates_f(MODULE_NPS_ID, &rates_f);
196
197}
198
200{
201
202 struct NedCoor_f ltp_pos;
203 VECT3_COPY(ltp_pos, fdm.ltpprz_pos);
204 stateSetPositionNed_f(MODULE_NPS_ID, &ltp_pos);
205
206 struct NedCoor_f ltp_speed;
207 VECT3_COPY(ltp_speed, fdm.ltpprz_ecef_vel);
208 stateSetSpeedNed_f(MODULE_NPS_ID, &ltp_speed);
209
210 struct NedCoor_f ltp_accel;
211 VECT3_COPY(ltp_accel, fdm.ltpprz_ecef_accel);
212 stateSetAccelNed_f(MODULE_NPS_ID, &ltp_accel);
213
214 // Here we make sure that ENU states are recalculated
216}
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.
Device independent GPS code (interface).
void gps_feed_value(void)
Definition gps_sim_nps.c:58
#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
void stateCalcPositionEnu_i(void)
Definition state.c:335
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
static void stateSetAirspeed_f(uint16_t id, float airspeed)
Set airspeed (float).
Definition state.h:1511
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.
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
Hardware independent API for actuators (servos, motor controllers).
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
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.