Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
fc_rotor.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2016 Hector Garcia de Marina
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, see
18  * <http://www.gnu.org/licenses/>.
19  *
20  */
22 #include "subsystems/abi.h"
23 #include "subsystems/datalink/datalink.h" // dl_buffer
24 #include "autopilot.h"
27 
28 void fc_rotor_init(void)
29 {
30 }
31 
32 void fc_read_msg(void)
33 {
34  struct FloatVect3 u;
35  uint8_t ac_id = DL_DESIRED_SETPOINT_ac_id(dl_buffer);
36 
37  if (ac_id == AC_ID) {
38  // 0: 2D control, 1: 3D control
39  uint8_t flag = DL_DESIRED_SETPOINT_flag(dl_buffer);
40 
41  u.x = DL_DESIRED_SETPOINT_ux(dl_buffer);
42  u.y = DL_DESIRED_SETPOINT_uy(dl_buffer);
43  u.z = DL_DESIRED_SETPOINT_uz(dl_buffer);
44 
45  AbiSendMsgACCEL_SP(ACCEL_SP_FCR_ID, flag, &u);
46 
47  if (flag == 0) {
48  // with 2D control, set flight altitude in integer ENU LTP frame
50  }
51  }
52 }
53 
uint8_t ac_id
Definition: sim_ap.c:45
#define POS_BFP_OF_REAL(_af)
#define ACCEL_SP_FCR_ID
Formation control algorithm for rotorcrafts.
Main include for ABI (AirBorneInterface).
int32_t nav_flight_altitude
Definition: navigation.c:111
void fc_read_msg(void)
Definition: fc_rotor.c:32
Paparazzi floating point algebra.
Core autopilot interface common to all firmwares.
Rotorcraft navigation functions.
unsigned char uint8_t
Definition: types.h:14
uint8_t dl_buffer[MSG_SIZE]
Definition: main_demo5.c:64
void fc_rotor_init(void)
Definition: fc_rotor.c:28