Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 "modules/core/abi.h"
24 #include "autopilot.h"
27 
28 void fc_rotor_init(void)
29 {
30 }
31 
32 void fc_read_msg(uint8_t *buf)
33 {
34  struct FloatVect3 u;
35  uint8_t ac_id = DL_DESIRED_SETPOINT_ac_id(buf);
36 
37  if (ac_id == AC_ID) {
38  // 0: 2D control, 1: 3D control
39  uint8_t flag = DL_DESIRED_SETPOINT_flag(buf);
40 
41  u.x = DL_DESIRED_SETPOINT_ux(buf);
42  u.y = DL_DESIRED_SETPOINT_uy(buf);
43  u.z = DL_DESIRED_SETPOINT_uz(buf);
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
49  nav_flight_altitude = POS_BFP_OF_REAL(u.z);
50  }
51  }
52 }
53 
Main include for ABI (AirBorneInterface).
#define ACCEL_SP_FCR_ID
Core autopilot interface common to all firmwares.
void fc_rotor_init(void)
Definition: fc_rotor.c:28
void fc_read_msg(uint8_t *buf)
Definition: fc_rotor.c:32
Formation control algorithm for rotorcrafts.
#define POS_BFP_OF_REAL(_af)
Paparazzi floating point algebra.
Rotorcraft navigation functions.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98