Paparazzi UAS  v5.15_devel-81-gd13dafb
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
intermcu_ap.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Freek van Tienen <freek.v.tienen@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 
27 #include "intermcu_ap.h"
28 #include "pprzlink/intermcu_msg.h"
30 #include "mcu_periph/uart.h"
31 
32 #include "subsystems/electrical.h"
33 #include "autopilot.h"
34 
35 #if COMMANDS_NB > 8
36 #error "INTERMCU UART CAN ONLY SEND 8 COMMANDS OR THE UART WILL BE OVERFILLED"
37 #endif
38 
39 
40 /* Main InterMCU defines */
41 struct intermcu_t intermcu = {
42  .device = (&((INTERMCU_LINK).device)),
43  .enabled = true,
44  .msg_available = false,
45 };
46 uint8_t imcu_msg_buf[128] __attribute__((aligned));
47 static struct fbw_status_t fbw_status;
48 static inline void intermcu_parse_msg(void (*rc_frame_handler)(void));
49 
50 #if IMCU_GPS
51 #include "std.h"
52 #include "subsystems/abi.h"
53 #include "subsystems/gps.h"
54 static struct GpsState gps_imcu;
55 #endif
56 
57 
58 #if PERIODIC_TELEMETRY
60 
61 /* Send FBW status */
62 static void send_status(struct transport_tx *trans, struct link_device *dev)
63 {
64  pprz_msg_send_FBW_STATUS(trans, dev, AC_ID,
67 }
68 #endif
69 
70 /* InterMCU initialization */
71 void intermcu_init(void)
72 {
73  pprz_transport_init(&intermcu.transport);
74 
75 #if IMCU_GPS
76  gps_imcu.fix = GPS_FIX_NONE;
77  gps_imcu.pdop = 0;
78  gps_imcu.sacc = 0;
79  gps_imcu.pacc = 0;
80  gps_imcu.cacc = 0;
81  gps_imcu.comp_id = GPS_IMCU_ID;
82 #endif
83 
84 #if PERIODIC_TELEMETRY
86 #endif
87 }
88 
89 /* Check for InterMCU loss */
91 {
92  /* Check for interMCU loss */
93  if (intermcu.time_since_last_frame >= INTERMCU_LOST_CNT) {
94  intermcu.status = INTERMCU_LOST;
95  } else {
96  intermcu.time_since_last_frame++;
97  }
98 
99 #if IMCU_GPS
100  RunOnceEvery(PERIODIC_FREQUENCY, gps_periodic_check(&gps_imcu));
101 #endif
102 }
103 
104 /* Enable or disable the communication of the InterMCU */
105 void intermcu_set_enabled(bool value)
106 {
107  intermcu.enabled = value;
108 }
109 
110 /* Send the actuators to the FBW */
111 void intermcu_set_actuators(pprz_t *command_values, uint8_t ap_mode __attribute__((unused)))
112 {
113  if (!intermcu.enabled) {
114  return;
115  }
116 
117  // Set the autopilot motors on status
118  if (autopilot_get_motors_on()) {
120  }
121 
122  // Send the message and reset cmd_status
123  pprz_msg_send_IMCU_COMMANDS(&(intermcu.transport.trans_tx), intermcu.device,
124  INTERMCU_AP, &intermcu.cmd_status, COMMANDS_NB, command_values); //TODO: Append more status
125  intermcu.cmd_status = 0;
126 }
127 
128 /* Send the spektrum Bind message */
130 {
131  if (intermcu.enabled) {
132  pprz_msg_send_IMCU_SPEKTRUM_SOFT_BIND(&(intermcu.transport.trans_tx), intermcu.device, INTERMCU_AP);
133  }
134 }
135 
136 /* Parse incomming InterMCU messages */
137 #pragma GCC diagnostic ignored "-Wcast-align"
138 static inline void intermcu_parse_msg(void (*rc_frame_handler)(void))
139 {
140  /* Parse the Inter MCU message */
141  uint8_t msg_id = pprzlink_get_msg_id(imcu_msg_buf);
142 #if PPRZLINK_DEFAULT_VER == 2
143  // Check that the message is really a intermcu message
144  if (pprzlink_get_msg_class_id(imcu_msg_buf) == DL_intermcu_CLASS_ID) {
145 #endif
146  switch (msg_id) {
147  case DL_IMCU_RADIO_COMMANDS: {
148  uint8_t i;
149  uint8_t size = DL_IMCU_RADIO_COMMANDS_values_length(imcu_msg_buf);
150  intermcu.status = DL_IMCU_RADIO_COMMANDS_status(imcu_msg_buf);
151  for (i = 0; i < size; i++) {
152  radio_control.values[i] = DL_IMCU_RADIO_COMMANDS_values(imcu_msg_buf)[i];
153  }
154 
158  rc_frame_handler();
159  break;
160  }
161 
162  case DL_IMCU_FBW_STATUS: {
163  fbw_status.rc_status = DL_IMCU_FBW_STATUS_rc_status(imcu_msg_buf);
164  fbw_status.frame_rate = DL_IMCU_FBW_STATUS_frame_rate(imcu_msg_buf);
165  fbw_status.mode = DL_IMCU_FBW_STATUS_mode(imcu_msg_buf);
166  fbw_status.electrical.vsupply = DL_IMCU_FBW_STATUS_vsupply(imcu_msg_buf);
167  fbw_status.electrical.current = DL_IMCU_FBW_STATUS_current(imcu_msg_buf);
168  break;
169  }
170 
171  #if TELEMETRY_INTERMCU
172  case DL_IMCU_DATALINK: {
173  uint8_t size = DL_IMCU_DATALINK_msg_length(imcu_msg_buf);
174  uint8_t *msg = DL_IMCU_DATALINK_msg(imcu_msg_buf);
175  telemetry_intermcu_on_msg(msg, size);
176  break;
177  }
178  #endif
179 
180  #if IMCU_GPS
181  case DL_IMCU_REMOTE_GPS: {
182  uint32_t now_ts = get_sys_time_usec();
183  gps_imcu.ecef_pos.x = DL_IMCU_REMOTE_GPS_ecef_x(imcu_msg_buf);
184  gps_imcu.ecef_pos.y = DL_IMCU_REMOTE_GPS_ecef_y(imcu_msg_buf);
185  gps_imcu.ecef_pos.z = DL_IMCU_REMOTE_GPS_ecef_z(imcu_msg_buf);
186  SetBit(gps_imcu.valid_fields, GPS_VALID_POS_ECEF_BIT);
187 
188  gps_imcu.lla_pos.alt = DL_IMCU_REMOTE_GPS_alt(imcu_msg_buf);
189  gps_imcu.hmsl = DL_IMCU_REMOTE_GPS_hmsl(imcu_msg_buf);
190  SetBit(gps_imcu.valid_fields, GPS_VALID_HMSL_BIT);
191 
192  gps_imcu.ecef_vel.x = DL_IMCU_REMOTE_GPS_ecef_xd(imcu_msg_buf);
193  gps_imcu.ecef_vel.y = DL_IMCU_REMOTE_GPS_ecef_yd(imcu_msg_buf);
194  gps_imcu.ecef_vel.z = DL_IMCU_REMOTE_GPS_ecef_zd(imcu_msg_buf);
195  SetBit(gps_imcu.valid_fields, GPS_VALID_VEL_ECEF_BIT);
196 
197  gps_imcu.course = DL_IMCU_REMOTE_GPS_course(imcu_msg_buf);
198  gps_imcu.gspeed = DL_IMCU_REMOTE_GPS_gspeed(imcu_msg_buf);
199  SetBit(gps_imcu.valid_fields, GPS_VALID_COURSE_BIT);
200 
201  gps_imcu.pacc = DL_IMCU_REMOTE_GPS_pacc(imcu_msg_buf);
202  gps_imcu.sacc = DL_IMCU_REMOTE_GPS_sacc(imcu_msg_buf);
203  gps_imcu.num_sv = DL_IMCU_REMOTE_GPS_numsv(imcu_msg_buf);
204  gps_imcu.fix = DL_IMCU_REMOTE_GPS_fix(imcu_msg_buf);
205 
206  // set gps msg time
207  gps_imcu.last_msg_ticks = sys_time.nb_sec_rem;
208  gps_imcu.last_msg_time = sys_time.nb_sec;
209 
210  if (gps_imcu.fix >= GPS_FIX_3D) {
211  gps_imcu.last_3dfix_ticks = sys_time.nb_sec_rem;
212  gps_imcu.last_3dfix_time = sys_time.nb_sec;
213  }
214 
215  AbiSendMsgGPS(GPS_IMCU_ID, now_ts, &gps_imcu);
216  break;
217  }
218 
219  #endif
220 
221  default:
222  break;
223  }
224 #if PPRZLINK_DEFAULT_VER == 2
225  }
226 #endif
227 }
228 #pragma GCC diagnostic pop
229 
230 /* Radio control event misused as InterMCU event for frame_handler */
231 void RadioControlEvent(void (*frame_handler)(void))
232 {
233  /* Parse incoming bytes */
234  if (intermcu.enabled) {
235  pprz_check_and_parse(intermcu.device, &intermcu.transport, imcu_msg_buf, &intermcu.msg_available);
236 
237  if (intermcu.msg_available) {
238  intermcu_parse_msg(frame_handler);
239  intermcu.msg_available = false;
240  }
241  }
242 }
uint8_t frame_rate
Definition: intermcu_ap.h:54
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
#define INTERMCU_LOST_CNT
Definition: intermcu.h:38
void RadioControlEvent(void(*frame_handler)(void))
Definition: intermcu_ap.c:231
static struct fbw_status_t fbw_status
Definition: intermcu_ap.c:47
void intermcu_set_enabled(bool value)
Definition: intermcu_ap.c:105
struct link_device * device
Device used for communication.
Definition: intermcu.h:71
Periodic telemetry system header (includes downlink utility and generated code).
uint8_t cmd_status
Command status information that is transfered (intermcu_cmd_status)
Definition: intermcu.h:77
bool autopilot_get_motors_on(void)
get motors status
Definition: autopilot.c:212
uint8_t status
Definition: radio_control.h:64
#define INTERMCU_SET_CMD_STATUS(_bit)
Definition: intermcu.h:65
struct intermcu_t intermcu
Definition: intermcu_ap.c:41
void telemetry_intermcu_on_msg(uint8_t *msg, uint8_t size)
int16_t pprz_t
Definition: paparazzi.h:6
Main include for ABI (AirBorneInterface).
void intermcu_periodic(void)
Definition: intermcu_ap.c:90
#define INTERMCU_AP
Definition: intermcu.h:34
enum intermcu_status status
Status of the INTERMCU.
Definition: intermcu.h:73
float vsupply
supply voltage in V
Definition: electrical.h:45
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:39
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:69
bool msg_available
If we have an InterMCU message.
Definition: intermcu.h:76
#define GPS_VALID_COURSE_BIT
Definition: gps.h:54
uint8_t imcu_msg_buf[128]
The InterMCU message buffer.
Definition: intermcu_ap.c:46
bool enabled
If the InterMCU communication is enabled.
Definition: intermcu.h:75
No interMCU communication anymore.
Definition: intermcu.h:46
static void intermcu_parse_msg(void(*rc_frame_handler)(void))
Definition: intermcu_ap.c:138
Interface for electrical status: supply voltage, current, battery status, etc.
data structure for GPS information
Definition: gps.h:87
The status of autopilot_motors_on.
Definition: intermcu.h:58
#define GPS_FIX_NONE
No GPS fix.
Definition: gps.h:37
Device independent GPS code (interface)
unsigned long uint32_t
Definition: types.h:18
#define GPS_VALID_HMSL_BIT
Definition: gps.h:53
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
struct RadioControl radio_control
Definition: radio_control.c:30
void intermcu_send_spektrum_bind(void)
Definition: intermcu_ap.c:129
struct pprz_transport transport
Transport over communication line (PPRZ)
Definition: intermcu.h:72
Rotorcraft Inter-MCU on the autopilot.
static void send_status(struct transport_tx *trans, struct link_device *dev)
Definition: intermcu_ap.c:62
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition: sys_time.h:73
#define RC_OK
Definition: radio_control.h:56
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
Core autopilot interface common to all firmwares.
void intermcu_init(void)
Definition: intermcu_ap.c:71
uint8_t rc_status
Definition: intermcu_ap.h:53
#define GPS_IMCU_ID
unsigned char uint8_t
Definition: types.h:14
uint8_t time_since_last_frame
Time since last frame.
Definition: intermcu.h:74
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
#define GPS_VALID_POS_ECEF_BIT
Definition: gps.h:48
uint8_t time_since_last_frame
Definition: radio_control.h:65
#define GPS_VALID_VEL_ECEF_BIT
Definition: gps.h:51
void intermcu_set_actuators(pprz_t *command_values, uint8_t ap_mode)
Definition: intermcu_ap.c:111
float current
current in A
Definition: electrical.h:46
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
uint8_t mode
Definition: intermcu_ap.h:55
struct Electrical electrical
Definition: intermcu_ap.h:56
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
void gps_periodic_check(struct GpsState *gps_s)
Periodic GPS check.
Definition: gps.c:264
uint8_t frame_cpt
Definition: radio_control.h:68