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