Paparazzi UAS  v5.12_stable-4-g9b43e9b
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->vsupply = data[3] + (data[4] << 8);
106  fbw_state->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  intermcu_tx_buff[3] = (uint8_t) fbw_state->vsupply;
151  intermcu_tx_buff[4] = (uint8_t)((fbw_state->vsupply & 0xff00) >> 8);
152  ppz_can_transmit(MSG_INTERMCU_FBW_STATUS_ID, intermcu_tx_buff, 5);
153 
154 #if defined RADIO_CONTROL || RADIO_CONTROL_AUTO1
155  // Copy the CHANNELS to the 2 CAN buffers
156  for (int i = 0; (i < RADIO_CONTROL_NB_CHANNEL) && (i < 4); i++) {
157  imcu_chan1.cmd[i] = fbw_state->channels[i];
158  }
159  for (int i = 0; (i < (RADIO_CONTROL_NB_CHANNEL - 4)) && (i < 4); i++) {
160  imcu_chan2.cmd[i] = fbw_state->channels[4 + i];
161  }
162 
163  if (bit_is_set(fbw_state->status, RC_OK)) {
166  }
167 #endif
168 }
169 #endif
170 
172 // STATUS LOGIC AND TELEMETRY
173 // Downlink FBW status from AP
174 
177 
178 
179 #ifdef AP
181 
182 #define RC_OK 0
183 #define RC_LOST 1
184 #define RC_REALLY_LOST 2
185 
186 
187 static void send_commands(struct transport_tx *trans, struct link_device *dev)
188 {
189  pprz_msg_send_COMMANDS(trans, dev, AC_ID, COMMANDS_NB, ap_state->commands);
190 }
191 
192 static void send_fbw_status(struct transport_tx *trans, struct link_device *dev)
193 {
194  uint8_t rc_status = 0;
195  uint8_t fbw_status = 0;
196  if (bit_is_set(fbw_state->status, STATUS_MODE_AUTO)) {
197  fbw_status = FBW_MODE_AUTO;
198  }
199  if (bit_is_set(fbw_state->status, STATUS_MODE_FAILSAFE)) {
200  fbw_status = FBW_MODE_FAILSAFE;
201  }
202  if (bit_is_set(fbw_state->status, STATUS_RADIO_REALLY_LOST)) {
203  rc_status = RC_REALLY_LOST;
204  } else if (bit_is_set(fbw_state->status, RC_OK)) {
205  rc_status = RC_OK;
206  } else {
207  rc_status = RC_LOST;
208  }
209  pprz_msg_send_FBW_STATUS(trans, dev, AC_ID,
210  &(rc_status), &(fbw_state->ppm_cpt), &(fbw_status), &(fbw_state->vsupply), &(fbw_state->current));
211 }
212 #endif
213 
214 void link_mcu_init(void)
215 {
217 
218 #ifdef AP
219 #if PERIODIC_TELEMETRY
220  // If FBW has not telemetry, then AP can send some of the info
223 #endif
224 #endif
225 }
226 
228 {
229  // No event function: CAN_RX_IRQ is called on reception of data
230 }
#define RC_LOST
Definition: radio_control.h:49
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
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:52
#define RC_REALLY_LOST
Definition: radio_control.h:50
#define RADIO_CONTROL_NB_CHANNEL
Definition: spektrum_arch.h:34
#define RC_OK
Definition: radio_control.h:48
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:326
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