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
link_mcu_can.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010-2012 The Paparazzi Team
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 
23 #include "link_mcu_can.h"
24 #include "mcu_periph/can.h"
25 #include "led.h"
26 
27 #include "subsystems/commands.h"
28 
29 
31 // INTERMCU CAN MESSAGES
32 
33 // Commands
34 #define MSG_INTERMCU_COMMAND_MASTER_ID 0x01
35 #define MSG_INTERMCU_COMMAND_EXTRA_ID 0x02
36 // Channels
37 #define MSG_INTERMCU_RADIO_LOW_ID 0x04
38 #define MSG_INTERMCU_RADIO_HIGH_ID 0x05
39 // Trim
40 #define MSG_INTERMCU_TRIM_ID 0x09
41 // Status
42 #define MSG_INTERMCU_FBW_STATUS_ID 0x10
43 
44 
45 union {
46  uint8_t data[8];
47  pprz_t cmd[4];
49 
50 #define INTERMCU_COMMAND(_intermcu_payload, nr) (pprz_t)((uint16_t)(*((uint8_t*)_intermcu_payload+0+(2*(nr)))|*((uint8_t*)_intermcu_payload+1+(2*(nr)))<<8))
51 
53 // READ MESSAGES
54 
55 
56 void link_mcu_on_can_msg(uint32_t id, uint8_t *data, int len);
57 void link_mcu_on_can_msg(uint32_t id, uint8_t *data, int len)
58 {
59 #if COMMANDS_NB > 8
60 #error "INTERMCU_CAN CAN ONLY SEND 4 OR 8 COMMANDS (packets of 8 bytes)"
61 #endif
62 #if RADIO_CONTROL_NB_CHANNEL > 8
63 #warning "INTERMCU_CAN CAN ONLY SEND 8 RADIO CHANNELS: CHANNELS 9 and higher will not be sent"
64 #endif
65  if (len) {} //Remove compile warning
66 
68  for (int i = 0; (i < 4) && (i < COMMANDS_NB); i++) {
69  ap_state->commands[i] = INTERMCU_COMMAND(data, i);
70  }
71 #ifdef LINK_MCU_LED
72  LED_TOGGLE(LINK_MCU_LED);
73 #endif
74  inter_mcu_received_ap = true;
75  }
76 
78  for (int i = 0; (i < 4) && (i < (COMMANDS_NB - 4)); i++) {
79  ap_state->commands[4 + i] = INTERMCU_COMMAND(data, i);
80  }
81  }
82 
83 
84  if (id == MSG_INTERMCU_TRIM_ID) {
85  ap_state->command_roll_trim = ((pprz_t) INTERMCU_COMMAND(data, 0));
86  ap_state->command_pitch_trim = ((pprz_t) INTERMCU_COMMAND(data, 1));
87  }
88 
89  if (id == MSG_INTERMCU_RADIO_LOW_ID) {
90  for (int i = 0; (i < 4) && (i < RADIO_CONTROL_NB_CHANNEL); i++) {
91  fbw_state->channels[i] = ((pprz_t)INTERMCU_COMMAND(data, i));
92  }
93  }
94 
95  if (id == MSG_INTERMCU_RADIO_HIGH_ID) {
96  for (int i = 0; (i < 4) && (i < (RADIO_CONTROL_NB_CHANNEL - 4)); i++) {
97  fbw_state->channels[4 + i] = ((pprz_t)INTERMCU_COMMAND(data, i));
98  }
99  }
100 
101  if ((id == MSG_INTERMCU_FBW_STATUS_ID) && (len == 5)) {
102  fbw_state->ppm_cpt = data[0];
103  fbw_state->status = data[1];
104  fbw_state->nb_err = data[2];
105  fbw_state->electrical.vsupply = (float)(data[3] + (data[4] << 8))/10.f;
106  fbw_state->electrical.current = 0;
107 
108 #ifdef LINK_MCU_LED
109  LED_TOGGLE(LINK_MCU_LED);
110 #endif
111  inter_mcu_received_fbw = true;
112  }
113 }
114 
116 // SEND MESSAGES
117 
118 
119 #ifdef AP
120 void link_mcu_send(void)
121 {
122  for (int i = 0; (i < COMMANDS_NB) && (i < 4); i++) {
123  imcu_cmd_mstr.cmd[i] = ap_state->commands[i];
124  }
125  for (int i = 0; (i < (COMMANDS_NB - 4)) && (i < 4); i++) {
126  imcu_cmd_ext.cmd[i] = ap_state->commands[4 + i];
127  }
128 
131 
132  imcu_trim.cmd[0] = ap_state->command_roll_trim;
133  imcu_trim.cmd[1] = ap_state->command_pitch_trim;
134  imcu_trim.cmd[2] = ap_state->command_yaw_trim;
135  RunOnceEvery(6, ppz_can_transmit(MSG_INTERMCU_TRIM_ID, imcu_trim.data, 6));
136 }
137 #endif
138 
139 #ifdef FBW
140 void link_mcu_periodic_task(void)
141 {
142  // Import: Prepare the next message for AP
143  inter_mcu_fill_fbw_state();
144 
145  // Transmit Status
146  uint8_t intermcu_tx_buff[8];
147  intermcu_tx_buff[0] = fbw_state->ppm_cpt;
148  intermcu_tx_buff[1] = fbw_state->status;
149  intermcu_tx_buff[2] = fbw_state->nb_err;
150  uint16_t vsupply = fbw_state->electrical.vsupply * 10;
151  intermcu_tx_buff[3] = (uint8_t) vsupply;
152  intermcu_tx_buff[4] = (uint8_t)(vsupply & 0xff00) >> 8);
153  ppz_can_transmit(MSG_INTERMCU_FBW_STATUS_ID, intermcu_tx_buff, 5);
154 
155 #if defined RADIO_CONTROL || RADIO_CONTROL_AUTO1
156  // Copy the CHANNELS to the 2 CAN buffers
157  for (int i = 0; (i < RADIO_CONTROL_NB_CHANNEL) && (i < 4); i++) {
158  imcu_chan1.cmd[i] = fbw_state->channels[i];
159  }
160  for (int i = 0; (i < (RADIO_CONTROL_NB_CHANNEL - 4)) && (i < 4); i++) {
161  imcu_chan2.cmd[i] = fbw_state->channels[4 + i];
162  }
163 
164  if (bit_is_set(fbw_state->status, RC_OK)) {
167  }
168 #endif
169 }
170 #endif
171 
173 // STATUS LOGIC AND TELEMETRY
174 // Downlink FBW status from AP
175 
178 
179 
180 #ifdef AP
182 
183 #define RC_OK 0
184 #define RC_LOST 1
185 #define RC_REALLY_LOST 2
186 
187 
188 static void send_commands(struct transport_tx *trans, struct link_device *dev)
189 {
190  pprz_msg_send_COMMANDS(trans, dev, AC_ID, COMMANDS_NB, ap_state->commands);
191 }
192 
193 static void send_fbw_status(struct transport_tx *trans, struct link_device *dev)
194 {
195  uint8_t rc_status = 0;
196  uint8_t fbw_status = 0;
197  if (bit_is_set(fbw_state->status, STATUS_MODE_AUTO)) {
198  fbw_status = FBW_MODE_AUTO;
199  }
200  if (bit_is_set(fbw_state->status, STATUS_MODE_FAILSAFE)) {
201  fbw_status = FBW_MODE_FAILSAFE;
202  }
203  if (bit_is_set(fbw_state->status, STATUS_RADIO_REALLY_LOST)) {
204  rc_status = RC_REALLY_LOST;
205  } else if (bit_is_set(fbw_state->status, RC_OK)) {
206  rc_status = RC_OK;
207  } else {
208  rc_status = RC_LOST;
209  }
210  pprz_msg_send_FBW_STATUS(trans, dev, AC_ID,
211  &(rc_status), &(fbw_state->ppm_cpt), &(fbw_status),
212  &(fbw_state->electrical.vsupply), &(fbw_state->electrical.current));
213 }
214 #endif
215 
216 void link_mcu_init(void)
217 {
219 
220 #ifdef AP
221 #if PERIODIC_TELEMETRY
222  // If FBW has not telemetry, then AP can send some of the info
225 #endif
226 #endif
227 }
228 
230 {
231  // No event function: CAN_RX_IRQ is called on reception of data
232 }
#define RC_LOST
Definition: radio_control.h:57
unsigned short uint16_t
Definition: types.h:16
static struct fbw_status_t fbw_status
Definition: intermcu_ap.c:47
Periodic telemetry system header (includes downlink utility and generated code).
int16_t pprz_t
Definition: paparazzi.h:6
volatile bool inter_mcu_received_ap
Definition: inter_mcu.c:41
void ppz_can_init(can_rx_callback_t callback)
Definition: can.c:32
struct fbw_state * fbw_state
Definition: inter_mcu.c:36
void(* can_rx_callback_t)(uint32_t id, uint8_t *buf, uint8_t len)
Definition: can.h:26
struct ap_state * ap_state
Definition: inter_mcu.c:37
#define RADIO_CONTROL_NB_CHANNEL
Definition: intermcu_ap.h:49
int ppz_can_transmit(uint32_t id, const uint8_t *buf, uint8_t len)
Definition: can.c:38
unsigned long uint32_t
Definition: types.h:18
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
Hardware independent code for commands handling.
#define LED_TOGGLE(i)
Definition: led_hw.h:53
#define RC_REALLY_LOST
Definition: radio_control.h:58
#define RC_OK
Definition: radio_control.h:56
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
volatile bool inter_mcu_received_fbw
Definition: inter_mcu.c:40
unsigned char uint8_t
Definition: types.h:14
static void send_fbw_status(struct transport_tx *trans, struct link_device *dev)
Definition: main_fbw.c:325
static void send_commands(struct transport_tx *trans, struct link_device *dev)
arch independent LED (Light Emitting Diodes) API
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46