-
Notifications
You must be signed in to change notification settings - Fork 1.1k
/
uart_cam_ctrl.c
182 lines (158 loc) · 4.92 KB
/
uart_cam_ctrl.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
176
177
178
179
180
181
182
/*
* Copyright (C) OpenUAS
*
* 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 modules/digital_cam/uart_cam_ctrl.c
* Control the camera via uart to chdk-ptp.
* Retrieve thumbnails
*/
#include "uart_cam_ctrl.h"
#include "generated/airframe.h"
// Include Standard Camera Control Interface
#include "modules/digital_cam/dc.h"
// Telemetry
#include "subsystems/datalink/telemetry.h"
#include BOARD_CONFIG
// Communication
#include "modules/digital_cam/catia/protocol.h"
#ifdef SITL
#include "modules/digital_cam/catia/serial.h"
#endif
#include "state.h"
#define CameraLinkDev (&(CAMERA_LINK).device)
#define CameraLinkTransmit(c) CameraLinkDev->put_byte(CameraLinkDev->periph, c)
#define CameraLinkChAvailable() CameraLinkDev->check_available(CameraLinkDev->periph)
#define CameraLinkGetch() CameraLinkGetch->get_byte(CameraLinkDev->periph)
union dc_shot_union dc_shot_msg;
union mora_status_union mora_status_msg;
int digital_cam_uart_status = 0;
int digital_cam_uart_thumbnails = 0;
#define THUMB_MSG_SIZE MORA_PAYLOAD_MSG_SIZE
#define THUMB_COUNT 10
static uint8_t thumbs[THUMB_COUNT][THUMB_MSG_SIZE];
static uint8_t thumb_pointer = 0;
#define ReadCameraBuffer() { \
while (CameraLinkChAvailable()) \
digital_cam_uart_parse(CameraLinkGetch()); \
}
void digital_cam_uart_event(void)
{
while (CameraLinkChAvailable()) {
parse_mora(&mora_protocol, CameraLinkGetch());
if (mora_protocol.msg_received) {
switch (mora_protocol.msg_id) {
case MORA_STATUS:
for (int i = 0; i < MORA_STATUS_MSG_SIZE; i++) {
mora_status_msg.bin[i] = mora_protocol.payload[i];
}
digital_cam_uart_status = mora_status_msg.data.shots;
break;
case MORA_PAYLOAD:
for (int i = 0; i < MORA_PAYLOAD_MSG_SIZE; i++) {
thumbs[thumb_pointer][i] = mora_protocol.payload[i];
}
break;
default:
break;
}
mora_protocol.msg_received = 0;
}
}
}
#if PERIODIC_TELEMETRY
static void send_thumbnails(struct transport_tx *trans, struct link_device *dev)
{
static int cnt = 0;
if (digital_cam_uart_thumbnails > 0) {
if (digital_cam_uart_thumbnails == 1) {
cnt++;
if (cnt > 1) {
cnt = 0;
return;
}
}
pprz_msg_send_PAYLOAD(trans, dev, AC_ID, THUMB_MSG_SIZE, thumbs[thumb_pointer]);
// Update the write/read pointer: if we receive a new thumb part, that will be sent, otherwise the oldest infor is repeated
thumb_pointer++;
if (thumb_pointer >= THUMB_COUNT) {
thumb_pointer = 0;
}
MoraHeader(MORA_BUFFER_EMPTY, 0);
MoraTrailer();
}
}
#endif
void digital_cam_uart_init(void)
{
// Call common DC init
dc_init();
digital_cam_uart_thumbnails = 0;
for (int t = 0; t < THUMB_COUNT; t++) {
for (int i = 0; i < THUMB_MSG_SIZE; i++) {
thumbs[t][i] = 0;
}
}
#if PERIODIC_TELEMETRY
register_periodic_telemetry(&telemetry_Ap, "PAYLOAD", send_thumbnails);
#endif
#ifdef SITL
serial_init("/dev/ttyUSB0");
#endif
}
void digital_cam_uart_periodic(void)
{
// Common DC Periodic task
dc_periodic_4Hz();
}
/* Command The Camera */
void dc_send_command(uint8_t cmd)
{
switch (cmd) {
case DC_SHOOT:
// Send Photo Position To Camera
dc_shot_msg.data.nr = dc_photo_nr + 1;
dc_shot_msg.data.lat = stateGetPositionLla_i()->lat;
dc_shot_msg.data.lon = stateGetPositionLla_i()->lon;
dc_shot_msg.data.alt = stateGetPositionLla_i()->alt;
dc_shot_msg.data.phi = stateGetNedToBodyEulers_i()->phi;
dc_shot_msg.data.theta = stateGetNedToBodyEulers_i()->theta;
dc_shot_msg.data.psi = stateGetNedToBodyEulers_i()->psi;
dc_shot_msg.data.vground = *stateGetHorizontalSpeedNorm_i();
dc_shot_msg.data.course = *stateGetHorizontalSpeedDir_i();
dc_shot_msg.data.groundalt = POS_BFP_OF_REAL(state.alt_agl_f);
MoraHeader(MORA_SHOOT, MORA_SHOOT_MSG_SIZE);
for (int i = 0; i < (MORA_SHOOT_MSG_SIZE); i++) {
MoraPutUint8(dc_shot_msg.bin[i]);
}
MoraTrailer();
dc_send_shot_position();
break;
case DC_TALLER:
break;
case DC_WIDER:
break;
case DC_ON:
break;
case DC_OFF:
break;
default:
break;
}
}