Skip to content

Commit 13ce2a9

Browse files
pablogs9pablogs9
authored
Creating a new recconect app and updating CF firmware to newer version (#2)
* Creating a new recconect app and updating CF firmware to newer version * Update -Werror * Renamed * Removed logging flag
1 parent 00271ee commit 13ce2a9

4 files changed

Lines changed: 210 additions & 2 deletions

File tree

‎apps/crazyflie_position_publisher_reconnection/app-colcon.meta‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
{
2+
"names": {
3+
"rmw_microxrcedds": {
4+
"cmake-args": [
5+
"-DRMW_UXRCE_TRANSPORT=custom_serial",
6+
"-DRMW_UXRCE_MAX_NODES=1",
7+
"-DRMW_UXRCE_MAX_PUBLISHERS=2",
8+
"-DRMW_UXRCE_MAX_SUBSCRIPTIONS=0",
9+
"-DRMW_UXRCE_MAX_SERVICES=0",
10+
"-DRMW_UXRCE_MAX_CLIENTS=0",
11+
"-DRMW_UXRCE_MAX_HISTORY=1",
12+
"-DRMW_UXRCE_XML_BUFFER_LENGTH=400"
13+
]
14+
}
15+
}
16+
}

‎apps/crazyflie_position_publisher_reconnection/app.c‎

Lines changed: 187 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,187 @@
1+
/*FreeRtos includes*/
2+
#include "FreeRTOS.h"
3+
#include "task.h"
4+
5+
#include <rcl/rcl.h>
6+
#include <geometry_msgs/msg/point32.h>
7+
#include "example_interfaces/srv/add_two_ints.h"
8+
#include <geometry_msgs/msg/twist.h>
9+
10+
#include <rcutils/allocator.h>
11+
12+
#include "config.h"
13+
#include "log.h"
14+
#include "crc.h"
15+
#include "worker.h"
16+
#include "num.h"
17+
#include "debug.h"
18+
#include "radiolink.h"
19+
#include <time.h>
20+
21+
#include "microrosapp.h"
22+
23+
#define RCCHECK(msg) if((rc != RCL_RET_OK)){DEBUG_PRINT("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)rc); goto clean;}
24+
#define RCSOFTCHECK(msg) if((rc != RCL_RET_OK)){DEBUG_PRINT("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)rc);}
25+
26+
static int pitchid, rollid, yawid;
27+
static int Xid, Yid, Zid;
28+
29+
float sign(float x){
30+
return (x >= 0) ? 1.0 : -1.0;
31+
}
32+
33+
void appMain(){
34+
while(1){
35+
absoluteUsedMemory = 0;
36+
usedMemory = 0;
37+
38+
// ####################### RADIO INIT #######################
39+
40+
int radio_connected = logGetVarId("radio", "isConnected");
41+
42+
while(!logGetUint(radio_connected)) vTaskDelay(100);
43+
DEBUG_PRINT("Radio connected\n");
44+
45+
// ####################### MICROROS INIT #######################
46+
47+
DEBUG_PRINT("Free heap pre uROS: %d bytes\n", xPortGetFreeHeapSize());
48+
vTaskDelay(50);
49+
50+
rcl_context_t context;
51+
rcl_init_options_t init_options;
52+
rcl_ret_t rc;
53+
rcl_ret_t rc_aux __attribute__((unused));
54+
55+
init_options = rcl_get_zero_initialized_init_options();
56+
rcl_allocator_t freeRTOS_allocator = rcutils_get_zero_initialized_allocator();
57+
freeRTOS_allocator.allocate = __crazyflie_allocate;
58+
freeRTOS_allocator.deallocate = __crazyflie_deallocate;
59+
freeRTOS_allocator.reallocate = __crazyflie_reallocate;
60+
freeRTOS_allocator.zero_allocate = __crazyflie_zero_allocate;
61+
62+
if (!rcutils_set_default_allocator(&freeRTOS_allocator)) {
63+
DEBUG_PRINT("Error on default allocators (line %d)\n",__LINE__);
64+
vTaskSuspend( NULL );
65+
}
66+
67+
rc = rcl_init_options_init(&init_options, rcutils_get_default_allocator());
68+
RCCHECK()
69+
70+
context = rcl_get_zero_initialized_context();
71+
72+
rc = rcl_init(0, NULL, &init_options, &context);
73+
RCCHECK()
74+
75+
rc = rcl_init_options_fini(&init_options);
76+
77+
rcl_node_t node = rcl_get_zero_initialized_node();
78+
rcl_node_options_t node_ops = rcl_node_get_default_options();
79+
80+
rc = rcl_node_init(&node, "crazyflie_node", "", &context, &node_ops);
81+
RCCHECK()
82+
83+
// Create publisher 1
84+
const char* drone_odom = "/drone/odometry";
85+
86+
rcl_publisher_t pub_odom = rcl_get_zero_initialized_publisher();
87+
const rosidl_message_type_support_t * pub_type_support_odom = ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Point32);
88+
rcl_publisher_options_t pub_opt_odom = rcl_publisher_get_default_options();
89+
pub_opt_odom.qos.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
90+
91+
92+
rc = rcl_publisher_init(
93+
&pub_odom,
94+
&node,
95+
pub_type_support_odom,
96+
drone_odom,
97+
&pub_opt_odom);
98+
99+
if(rc != RCL_RET_OK){
100+
rc_aux = rcl_node_fini(&node);
101+
}
102+
RCCHECK()
103+
104+
// Create publisher 2
105+
const char* drone_attitude = "/drone/attitude";
106+
107+
rcl_publisher_t pub_attitude = rcl_get_zero_initialized_publisher();
108+
const rosidl_message_type_support_t * pub_type_support_att = ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Point32);
109+
rcl_publisher_options_t pub_opt_att = rcl_publisher_get_default_options();
110+
pub_opt_att.qos.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
111+
112+
rc = rcl_publisher_init(
113+
&pub_attitude,
114+
&node,
115+
pub_type_support_att,
116+
drone_attitude,
117+
&pub_opt_att);
118+
119+
if(rc != RCL_RET_OK){
120+
rc_aux = rcl_publisher_fini(&pub_odom, &node);
121+
rc_aux = rcl_node_fini(&node);
122+
}
123+
RCCHECK()
124+
125+
// Init messages
126+
geometry_msgs__msg__Point32 pose;
127+
geometry_msgs__msg__Point32__init(&pose);
128+
geometry_msgs__msg__Point32 odom;
129+
geometry_msgs__msg__Point32__init(&odom);
130+
131+
//Get pitch, roll and yaw value
132+
pitchid = logGetVarId("stateEstimate", "pitch");
133+
rollid = logGetVarId("stateEstimate", "roll");
134+
yawid = logGetVarId("stateEstimate", "yaw");
135+
136+
//Get X,Y and Z value
137+
Xid = logGetVarId("stateEstimate", "x");
138+
Yid = logGetVarId("stateEstimate", "y");
139+
Zid = logGetVarId("stateEstimate", "z");
140+
141+
DEBUG_PRINT("Free heap post uROS configuration: %d bytes\n", xPortGetFreeHeapSize());
142+
DEBUG_PRINT("uROS Used Memory %d bytes\n", usedMemory);
143+
DEBUG_PRINT("uROS Absolute Used Memory %d bytes\n", absoluteUsedMemory);
144+
145+
// ####################### MAIN LOOP #######################
146+
147+
// static P2PPacket pk;
148+
// pk.port = 0;
149+
// pk.size = 11;
150+
// memcpy(pk.data, "Hello World", 11);
151+
// radiolinkSendP2PPacketBroadcast(&pk);
152+
153+
radiolinkSetPowerDbm(-50);
154+
155+
while(logGetUint(radio_connected)){
156+
157+
pose.x = logGetFloat(pitchid);
158+
pose.y = logGetFloat(rollid);
159+
pose.z = logGetFloat(yawid);
160+
odom.x = logGetFloat(Xid);
161+
odom.y = logGetFloat(Yid);
162+
odom.z = logGetFloat(Zid);
163+
164+
// Debug
165+
// int radio_rssi = logGetVarId("radio", "rssi");
166+
// odom.x = logGetFloat(radio_rssi);
167+
// odom.y = xPortGetFreeHeapSize();
168+
169+
rc = rcl_publish( &pub_attitude, (const void *) &pose, NULL);
170+
RCSOFTCHECK()
171+
172+
rc = rcl_publish( &pub_odom, (const void *) &odom, NULL);
173+
RCSOFTCHECK()
174+
175+
vTaskDelay(10/portTICK_RATE_MS);
176+
}
177+
178+
rc = rcl_publisher_fini(&pub_odom, &node);
179+
rc = rcl_publisher_fini(&pub_attitude, &node);
180+
rc = rcl_node_fini(&node);
181+
clean:
182+
rc = rcl_shutdown(&context);
183+
DEBUG_PRINT("Connection lost, retriying\n");
184+
}
185+
186+
vTaskSuspend( NULL );
187+
}

‎microros_crazyflie21_extensions/Makefile‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@ EXTENSIONS_DIR = $(TOPFOLDER)/freertos_apps/microros_crazyflie21_extensions
99
SRC_DIR = $(EXTENSIONS_DIR)/src
1010
CRAZYFLIE_BASE = $(TOPFOLDER)/crazyflie_firmware
1111

12-
ARCHCPUFLAGS = -DARM_MATH_CM4 -D__FPU_PRESENT=1 -D__TARGET_FPU_VFP -mfloat-abi=hard -mfpu=fpv4-sp-d16 -mcpu=cortex-m4 -mthumb -ffunction-sections -fdata-sections
12+
ARCHCPUFLAGS = -DARM_MATH_CM4 -D__FPU_PRESENT=1 -D__TARGET_FPU_VFP -mfloat-abi=hard -mfpu=fpv4-sp-d16 -mcpu=cortex-m4 -mthumb -ffunction-sections -fdata-sections
1313

1414
ifeq ($(DEBUG), 1)
1515
ARCHCPUFLAGS += -O0 -g3
@@ -67,7 +67,7 @@ INCLUDES += $(MICROROS_INCLUDES)
6767
VPATH += $(MICROROS_POSIX_FREERTOS_OBJECTS_VPATH)
6868
VPATH += $(SRC_DIR)/
6969
VPATH += $(UROS_APP_FOLDER)/
70-
CFLAGS += -DFREERTOS_HEAP_SIZE=60000
70+
CFLAGS += -DFREERTOS_HEAP_SIZE=17000
7171
CROSS_COMPILE ?= $(TOPFOLDER)/toolchain/bin/arm-none-eabi-
7272

7373
include $(CRAZYFLIE_BASE)/Makefile

‎microros_crazyflie21_extensions/src/crazyflie_transport.c‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33
#include <unistd.h>
44
#include <stdio.h>
55
#include <string.h>
6+
#include <watchdog.h>
67

78
#include "crtp.h"
89
#include "debug.h"
@@ -44,6 +45,8 @@ size_t uxr_write_serial_data_platform(uxrSerialPlatform* platform, uint8_t* buf,
4445
crtpSendPacket(&send_pkg);
4546
}
4647

48+
watchdogReset();
49+
4750
return index;
4851
}
4952

@@ -93,6 +96,8 @@ size_t uxr_read_serial_data_platform(uxrSerialPlatform* platform, uint8_t* buf,
9396
written = crpt_index;
9497
crpt_index = 0;
9598
}
99+
100+
watchdogReset();
96101

97102
// DEBUG_PRINT("Retrived %d Flushed %d Max %d Current %d\n", retrieved, written, crpt_index_max, crpt_index);
98103
return written;

0 commit comments

Comments
 (0)