Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 "-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 
41 static 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 
65 void 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.
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
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 ...
enum MAVLINK_MISSION_MGR_STATES state
@ STATE_SEND_LIST
@ STATE_SEND_ITEM
@ STATE_IDLE
static int seq
Definition: nps_ivy.c:32
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98