-
Notifications
You must be signed in to change notification settings - Fork 1.1k
/
intermcu_ap.c
175 lines (150 loc) · 5.07 KB
/
intermcu_ap.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
/*
* Copyright (C) 2015 Freek van Tienen <freek.v.tienen@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
/** @file subsystems/intermcu/intermcu_ap.c
* @brief Rotorcraft Inter-MCU on the autopilot
*/
#include "intermcu_ap.h"
#include "pprzlink/intermcu_msg.h"
#include "subsystems/radio_control.h"
#include "mcu_periph/uart.h"
#include "subsystems/electrical.h"
#include "autopilot.h"
#if COMMANDS_NB > 8
#error "INTERMCU UART CAN ONLY SEND 8 COMMANDS OR THE UART WILL BE OVERFILLED"
#endif
/* Main InterMCU defines */
struct intermcu_t intermcu = {
.device = (&((INTERMCU_LINK).device)),
.enabled = true,
.msg_available = false,
};
uint8_t imcu_msg_buf[128] __attribute__((aligned)); ///< The InterMCU message buffer
static struct fbw_status_t fbw_status;
static inline void intermcu_parse_msg(void (*rc_frame_handler)(void));
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
/* Send FBW status */
static void send_status(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_FBW_STATUS(trans, dev, AC_ID,
&fbw_status.rc_status, &fbw_status.frame_rate, &fbw_status.mode, &fbw_status.vsupply,
&fbw_status.current);
}
#endif
/* InterMCU initialization */
void intermcu_init(void)
{
pprz_transport_init(&intermcu.transport);
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_FBW_STATUS, send_status);
#endif
}
/* Check for InterMCU loss */
void intermcu_periodic(void)
{
/* Check for interMCU loss */
if (intermcu.time_since_last_frame >= INTERMCU_LOST_CNT) {
intermcu.status = INTERMCU_LOST;
} else {
intermcu.time_since_last_frame++;
}
}
/* Enable or disable the communication of the InterMCU */
void intermcu_set_enabled(bool value)
{
intermcu.enabled = value;
}
/* Send the actuators to the FBW */
void intermcu_set_actuators(pprz_t *command_values, uint8_t ap_mode __attribute__((unused)))
{
if (!intermcu.enabled) {
return;
}
// Set the autopilot motors on status
if (autopilot_motors_on) {
INTERMCU_SET_CMD_STATUS(INTERMCU_CMD_MOTORS_ON);
}
// Send the message and reset cmd_status
pprz_msg_send_IMCU_COMMANDS(&(intermcu.transport.trans_tx), intermcu.device,
INTERMCU_AP, &intermcu.cmd_status, COMMANDS_NB, command_values); //TODO: Append more status
intermcu.cmd_status = 0;
}
/* Send the spektrum Bind message */
void intermcu_send_spektrum_bind(void)
{
if (intermcu.enabled) {
pprz_msg_send_IMCU_SPEKTRUM_SOFT_BIND(&(intermcu.transport.trans_tx), intermcu.device, INTERMCU_AP);
}
}
/* Parse incomming InterMCU messages */
#pragma GCC diagnostic ignored "-Wcast-align"
static inline void intermcu_parse_msg(void (*rc_frame_handler)(void))
{
/* Parse the Inter MCU message */
uint8_t msg_id = imcu_msg_buf[1];
switch (msg_id) {
case DL_IMCU_RADIO_COMMANDS: {
uint8_t i;
uint8_t size = DL_IMCU_RADIO_COMMANDS_values_length(imcu_msg_buf);
intermcu.status = DL_IMCU_RADIO_COMMANDS_status(imcu_msg_buf);
for (i = 0; i < size; i++) {
radio_control.values[i] = DL_IMCU_RADIO_COMMANDS_values(imcu_msg_buf)[i];
}
radio_control.frame_cpt++;
radio_control.time_since_last_frame = 0;
radio_control.status = RC_OK;
rc_frame_handler();
break;
}
case DL_IMCU_FBW_STATUS: {
fbw_status.rc_status = DL_IMCU_FBW_STATUS_rc_status(imcu_msg_buf);
fbw_status.frame_rate = DL_IMCU_FBW_STATUS_frame_rate(imcu_msg_buf);
fbw_status.mode = DL_IMCU_FBW_STATUS_mode(imcu_msg_buf);
fbw_status.vsupply = DL_IMCU_FBW_STATUS_vsupply(imcu_msg_buf);
fbw_status.current = DL_IMCU_FBW_STATUS_current(imcu_msg_buf);
break;
}
#if TELEMETRY_INTERMCU
case DL_IMCU_DATALINK: {
uint8_t size = DL_IMCU_DATALINK_msg_length(imcu_msg_buf);
uint8_t *msg = DL_IMCU_DATALINK_msg(imcu_msg_buf);
telemetry_intermcu_on_msg(0, msg, size);
break;
}
#endif
default:
break;
}
}
#pragma GCC diagnostic pop
/* Radio control event misused as InterMCU event for frame_handler */
void RadioControlEvent(void (*frame_handler)(void))
{
/* Parse incoming bytes */
if (intermcu.enabled) {
pprz_check_and_parse(intermcu.device, &intermcu.transport, imcu_msg_buf, &intermcu.msg_available);
if (intermcu.msg_available) {
intermcu_parse_msg(frame_handler);
intermcu.msg_available = false;
}
}
}