Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
copilot_common.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2016 2017 Michal Podhradsky <http://github.com/podhrmic>
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  */
46 
50 
54 
55 PPRZ_MUTEX(copilot_cam_snapshot_mtx);
56 PPRZ_MUTEX(copilot_cam_payload_mtx);
57 PPRZ_MUTEX(copilot_status_mtx);
58 
60 void copilot_init(void)
61 {
62  send_cam_snapshot = false;
63  send_cam_payload = false;
64  send_copilot_status = false;
65 
66  memset(&cam_payload, 0, sizeof(cam_payload));
67  memset(&cam_snapshot, 0, sizeof(cam_snapshot));
68  memset(&copilot_status, 0, sizeof(copilot_status));
69 
70  PPRZ_MUTEX_INIT(copilot_cam_snapshot_mtx);
71  PPRZ_MUTEX_INIT(copilot_cam_payload_mtx);
72  PPRZ_MUTEX_INIT(copilot_status_mtx);
73 }
74 
76 void copilot_periodic(void)
77 {
78  PPRZ_MUTEX_LOCK(copilot_cam_snapshot_mtx);
79  if (send_cam_snapshot) {
80  // send down to GCS
81  DOWNLINK_SEND_CAMERA_SNAPSHOT(DefaultChannel, DefaultDevice,
85 
86  send_cam_snapshot = false;
87  }
88  PPRZ_MUTEX_UNLOCK(copilot_cam_snapshot_mtx);
89 
90  PPRZ_MUTEX_LOCK(copilot_cam_payload_mtx);
91  if (send_cam_payload) {
92  // NOTE: to send the message over the EXTRA_DL port
93  // use "DOWNLINK_SEND_CAMERA_PAYLOAD(extra_pprz_tp, EXTRA_DOWNLINK_DEVICE,"
94 
95  // send down to GCS
96  DOWNLINK_SEND_CAMERA_PAYLOAD(DefaultChannel, DefaultDevice,
99 
100  send_cam_payload = false;
101  }
102  PPRZ_MUTEX_UNLOCK(copilot_cam_payload_mtx);
103 
104  PPRZ_MUTEX_LOCK(copilot_status_mtx);
105  // send down to GCS
106  if (send_copilot_status) {
107  DOWNLINK_SEND_COPILOT_STATUS(DefaultChannel, DefaultDevice,
111 
112  send_copilot_status = false;
113  }
114  PPRZ_MUTEX_UNLOCK(copilot_status_mtx);
115 
116 }
117 
128 {
129  PPRZ_MUTEX_LOCK(copilot_cam_snapshot_mtx);
130 
131  // copy CAMERA_SNAPSHOT message and mark it to be sent
132  cam_snapshot.cam_id = DL_CAMERA_SNAPSHOT_DL_camera_id(buf);
133  cam_snapshot.cam_state = DL_CAMERA_SNAPSHOT_DL_camera_state(buf);
134  cam_snapshot.snapshot_num = DL_CAMERA_SNAPSHOT_DL_snapshot_image_number(buf);
135  cam_snapshot.snapshot_valid = DL_CAMERA_SNAPSHOT_DL_snapshot_valid(buf);
136  cam_snapshot.lens_temp = DL_CAMERA_SNAPSHOT_DL_lens_temp(buf);
137  cam_snapshot.array_temp = DL_CAMERA_SNAPSHOT_DL_array_temp(buf);
138 
139  send_cam_snapshot = true;
140 
141  PPRZ_MUTEX_UNLOCK(copilot_cam_snapshot_mtx);
142 }
143 
148 {
149  PPRZ_MUTEX_LOCK(copilot_cam_payload_mtx);
150 
151  cam_payload.timestamp = DL_CAMERA_PAYLOAD_DL_timestamp(buf);
152  cam_payload.used_mem = DL_CAMERA_PAYLOAD_DL_used_memory(buf);
153  cam_payload.used_disk = DL_CAMERA_PAYLOAD_DL_used_disk(buf);
154  cam_payload.door_status = DL_CAMERA_PAYLOAD_DL_door_status(buf);
155  cam_payload.error_code = DL_CAMERA_PAYLOAD_DL_error_code(buf);
156 
157  send_cam_payload = true;
158 
159  PPRZ_MUTEX_UNLOCK(copilot_cam_payload_mtx);
160 }
161 
166 {
167  PPRZ_MUTEX_LOCK(copilot_status_mtx);
168 
169  copilot_status.timestamp = DL_COPILOT_STATUS_DL_timestamp(buf);
170  copilot_status.used_mem = DL_COPILOT_STATUS_DL_used_memory(buf);
171  copilot_status.used_disk = DL_COPILOT_STATUS_DL_used_disk(buf);
172  copilot_status.status = DL_COPILOT_STATUS_DL_status(buf);
173  copilot_status.error_code = DL_COPILOT_STATUS_DL_error_code(buf);
174 
175  send_copilot_status = true;
176 
177  PPRZ_MUTEX_UNLOCK(copilot_status_mtx);
178 }
179 
184 void copilot_parse_payload_command_dl(uint8_t *buf __attribute__((unused)))
185 {
186 #if FORWARD_PAYLOAD
187 MESSAGE("Forwarding PAYLOAD_COMMAND messages.")
188  // Check if message was for us
189  if (DL_PAYLOAD_COMMAND_ac_id(buf) == AC_ID) {
190  uint8_t len = DL_PAYLOAD_COMMAND_command_length(buf);
191 
192  if (buf == extra_dl_buffer) {
193  // Message came from extra_dl, forward to GCS via telemetry
194  DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice,
195  len, DL_PAYLOAD_COMMAND_command(buf));
196  }
197 
198  if (buf == dl_buffer) {
199  // Message came from GCS/Telemetry, forward to extra_dl
200  DOWNLINK_SEND_PAYLOAD(extra_pprz_tp, EXTRA_DOWNLINK_DEVICE,
201  len, DL_PAYLOAD_COMMAND_command(buf));
202  }
203  }
204 #endif /* FORWARD_PAYLOAD */
205 }
CameraSnapshot::snapshot_valid
uint8_t snapshot_valid
Definition: copilot.h:67
CameraPayload::timestamp
float timestamp
Definition: copilot.h:56
dl_buffer
uint8_t dl_buffer[MSG_SIZE]
Definition: main_demo5.c:64
copilot.h
CopilotStatus::error_code
uint8_t error_code
Definition: copilot.h:77
PPRZ_MUTEX_INIT
#define PPRZ_MUTEX_INIT(_mtx)
Definition: pprz_mutex.h:45
CopilotStatus::timestamp
float timestamp
Definition: copilot.h:73
CopilotStatus::used_mem
uint8_t used_mem
Definition: copilot.h:74
CopilotStatus::used_disk
uint8_t used_disk
Definition: copilot.h:75
CameraPayload::door_status
uint8_t door_status
Definition: copilot.h:59
copilot_status
struct CopilotStatus copilot_status
Definition: copilot_common.c:53
send_copilot_status
bool send_copilot_status
Definition: copilot_common.c:49
send_cam_payload
bool send_cam_payload
Definition: copilot_common.c:48
copilot_parse_cam_snapshot_dl
void copilot_parse_cam_snapshot_dl(uint8_t *buf)
copy CAMERA_SNAPSHOT message and mark it to be sent
Definition: copilot_common.c:127
send_cam_snapshot
bool send_cam_snapshot
Definition: copilot_common.c:47
copilot_periodic
void copilot_periodic(void)
Periodic function.
Definition: copilot_common.c:76
copilot_parse_copilot_status_dl
void copilot_parse_copilot_status_dl(uint8_t *buf)
copy COPILOT_STATUS message and mark it to be sent
Definition: copilot_common.c:165
PPRZ_MUTEX_UNLOCK
#define PPRZ_MUTEX_UNLOCK(_mtx)
Definition: pprz_mutex.h:47
copilot_init
void copilot_init(void)
Init function.
Definition: copilot_common.c:60
telemetry.h
copilot_parse_payload_command_dl
void copilot_parse_payload_command_dl(uint8_t *buf)
Pass through PAYLOAD_COMMAND message and send it as PAYLOAD msg over telemetry.
Definition: copilot_common.c:184
CameraSnapshot::cam_state
uint8_t cam_state
Definition: copilot.h:65
CameraSnapshot
Definition: copilot.h:63
CameraPayload
Definition: copilot.h:55
CopilotStatus::status
uint8_t status
Definition: copilot.h:76
uint8_t
unsigned char uint8_t
Definition: types.h:14
CameraPayload::error_code
uint8_t error_code
Definition: copilot.h:60
copilot_parse_cam_payload_dl
void copilot_parse_cam_payload_dl(uint8_t *buf)
copy CAMERA_PAYLOAD message and mark it to be sent
Definition: copilot_common.c:147
CameraSnapshot::cam_id
uint16_t cam_id
Definition: copilot.h:64
extra_pprz_tp
struct pprz_transport extra_pprz_tp
Definition: extra_pprz_dl.c:57
cam_payload
struct CameraPayload cam_payload
Definition: copilot_common.c:51
PPRZ_MUTEX_LOCK
#define PPRZ_MUTEX_LOCK(_mtx)
Definition: pprz_mutex.h:46
PPRZ_MUTEX
PPRZ_MUTEX(copilot_cam_snapshot_mtx)
extra_dl_buffer
uint8_t extra_dl_buffer[MSG_SIZE]
Definition: extra_pprz_dl.c:55
CameraSnapshot::lens_temp
float lens_temp
Definition: copilot.h:68
CameraSnapshot::array_temp
float array_temp
Definition: copilot.h:69
CameraPayload::used_mem
uint8_t used_mem
Definition: copilot.h:57
cam_snapshot
struct CameraSnapshot cam_snapshot
Definition: copilot_common.c:52
CameraPayload::used_disk
uint8_t used_disk
Definition: copilot.h:58
CameraSnapshot::snapshot_num
uint16_t snapshot_num
Definition: copilot.h:66
CopilotStatus
Definition: copilot.h:72