Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
blocks.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2015 Lodewijk Sikkel <l.n.c.sikkel@tudelft.nl>
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
26
30
31// include mavlink headers, but ignore some warnings
32#pragma GCC diagnostic push
33#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
34#pragma GCC diagnostic ignored "-Wswitch-default"
35#include "mavlink/paparazzi/mavlink.h"
36#pragma GCC diagnostic pop
37
39#include "generated/flight_plan.h"
40
41static void mavlink_send_block_count(void)
42{
43 mavlink_msg_script_count_send(MAVLINK_COMM_0, mission_mgr.rem_sysid, mission_mgr.rem_compid, NB_BLOCK);
45 MAVLINK_DEBUG("Sent BLOCK_COUNT message: count %i\n", NB_BLOCK);
46}
47
49{
50 if (seq < NB_BLOCK) { // Due to indexing
51 static const char *blocks[] = FP_BLOCKS;
52 char block_name[50];
53 strncpy(block_name, blocks[seq], 49); // String containing the name of the block
54 // make sure the string is null terminated if longer than 49chars
55 block_name[49] = 0;
56 mavlink_msg_script_item_send(MAVLINK_COMM_0, mission_mgr.rem_sysid, mission_mgr.rem_compid,
57 seq, block_name);
59 MAVLINK_DEBUG("Sent BLOCK_ITEM message: seq %i, name %s\n", seq, block_name);
60 } else {
61 MAVLINK_DEBUG("ERROR: Block index %i out of bounds\n", seq);
62 }
63}
64
65void mavlink_block_message_handler(const mavlink_message_t *msg)
66{
67 switch (msg->msgid) {
68 /* request for script/block list, answer with number of blocks */
69 case MAVLINK_MSG_ID_SCRIPT_REQUEST_LIST: {
70 MAVLINK_DEBUG("Received BLOCK_REQUEST_LIST message\n");
71 mavlink_script_request_list_t block_request_list_msg;
72 mavlink_msg_script_request_list_decode(msg, &block_request_list_msg);
73 if (block_request_list_msg.target_system == mavlink_system.sysid) {
74 if (mission_mgr.state == STATE_IDLE) {
75 if (NB_BLOCK > 0) {
77 MAVLINK_DEBUG("State: %d\n", mission_mgr.state);
78 mission_mgr.seq = 0;
79 mission_mgr.rem_sysid = msg->sysid;
80 mission_mgr.rem_compid = msg->compid;
81 }
83
84 // Register the timeout timer (it is continuous so it needs to be cancelled after triggering)
86 } else {
87 // TODO: Handle case when the state is not IDLE
88 }
89 } else {
90 // TODO: Handle remote system id mismatch
91 }
92
93 break;
94 }
95
96 /* request script/block, answer with SCRIPT_ITEM (block) */
97 case MAVLINK_MSG_ID_SCRIPT_REQUEST: {
98 MAVLINK_DEBUG("Received BLOCK_REQUEST message\n");
99 mavlink_script_request_t block_request_msg;
100 mavlink_msg_script_request_decode(msg, &block_request_msg);
101 if (block_request_msg.target_system == mavlink_system.sysid) {
102 // Handle only cases in which the entire list is request, the block is sent again,
103 // or the next block was sent
104 if ((mission_mgr.state == STATE_SEND_LIST && block_request_msg.seq == 0) ||
105 (mission_mgr.state == STATE_SEND_ITEM && (block_request_msg.seq == mission_mgr.seq
106 || block_request_msg.seq == mission_mgr.seq + 1))) {
107 // Cancel the timeout timer
109
111 MAVLINK_DEBUG("State: %d\n", mission_mgr.state);
112 mission_mgr.seq = block_request_msg.seq;
113
115
116 // Register the timeout timer
118 } else {
119 // TODO: Handle cases for which the above condition does not hold
120 }
121 } else {
122 // TODO: Handle remote system id mismatch
123 }
124
125 break;
126 }
127
128 /* got script item, change block */
129 case MAVLINK_MSG_ID_SCRIPT_ITEM: {
130 MAVLINK_DEBUG("Received BLOCK_ITEM message\n");
131 mavlink_script_item_t block_item_msg;
132 mavlink_msg_script_item_decode(msg, &block_item_msg);
133 if (block_item_msg.target_system == mavlink_system.sysid) {
134 // Only handle incoming block item messages if they a not currently being sent
135 if (mission_mgr.state == STATE_IDLE) {
136 nav_goto_block((uint8_t)block_item_msg.seq); // Set the current block
137 }
138 }
139 }
140
141 default:
142 break;
143 }
144}
void mavlink_send_block(uint16_t seq)
Definition blocks.c:48
void mavlink_block_message_handler(const mavlink_message_t *msg)
Definition blocks.c:65
static void mavlink_send_block_count(void)
Definition blocks.c:41
PPRZ specific mission block implementation.
void nav_goto_block(uint8_t b)
Common flight_plan functions shared between fixedwing and rotorcraft.
void mavlink_mission_set_timer(void)
void mavlink_mission_cancel_timer(void)
Common functions used within the mission library, blocks and waypoints cannot be send simultaneously ...
@ STATE_SEND_LIST
@ STATE_SEND_ITEM
@ STATE_IDLE
static int seq
Definition nps_ivy.c:31
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.