diff --git a/msg/navdata_adc_data_frame.msg b/msg/navdata_adc_data_frame.msg index 8a7bb3d..f4d2b88 100644 --- a/msg/navdata_adc_data_frame.msg +++ b/msg/navdata_adc_data_frame.msg @@ -1,5 +1,6 @@ Header header -float32 drone_timeuint16 tag +float64 drone_time +uint16 tag uint16 size uint32 version uint8[] data_frame diff --git a/msg/navdata_altitude.msg b/msg/navdata_altitude.msg index d9c50f4..63e6122 100644 --- a/msg/navdata_altitude.msg +++ b/msg/navdata_altitude.msg @@ -1,5 +1,6 @@ Header header -float32 drone_timeuint16 tag +float64 drone_time +uint16 tag uint16 size int32 altitude_vision float32 altitude_vz @@ -7,7 +8,7 @@ int32 altitude_ref int32 altitude_raw float32 obs_accZ float32 obs_alt -ardrone_autonomy::vector31 obs_x +vector31 obs_x uint32 obs_state -ardrone_autonomy::vector21 est_vb +vector21 est_vb uint32 est_state diff --git a/msg/navdata_demo.msg b/msg/navdata_demo.msg index 72a7523..5b1cb6f 100644 --- a/msg/navdata_demo.msg +++ b/msg/navdata_demo.msg @@ -1,5 +1,6 @@ Header header -float32 drone_timeuint16 tag +float64 drone_time +uint16 tag uint16 size uint32 ctrl_state uint32 vbat_flying_percentage diff --git a/msg/navdata_euler_angles.msg b/msg/navdata_euler_angles.msg index e450caa..40f91a1 100644 --- a/msg/navdata_euler_angles.msg +++ b/msg/navdata_euler_angles.msg @@ -1,5 +1,6 @@ Header header -float32 drone_timeuint16 tag +float64 drone_time +uint16 tag uint16 size float32 theta_a float32 phi_a diff --git a/msg/navdata_games.msg b/msg/navdata_games.msg index a37a19e..043ae04 100644 --- a/msg/navdata_games.msg +++ b/msg/navdata_games.msg @@ -1,5 +1,6 @@ Header header -float32 drone_timeuint16 tag +float64 drone_time +uint16 tag uint16 size uint32 double_tap_counter uint32 finish_line_counter diff --git a/msg/navdata_gyros_offsets.msg b/msg/navdata_gyros_offsets.msg index c612143..e69bddd 100644 --- a/msg/navdata_gyros_offsets.msg +++ b/msg/navdata_gyros_offsets.msg @@ -1,4 +1,5 @@ Header header -float32 drone_timeuint16 tag +float64 drone_time +uint16 tag uint16 size float32[] offset_g diff --git a/msg/navdata_hdvideo_stream.msg b/msg/navdata_hdvideo_stream.msg index 45bf59c..4651b00 100644 --- a/msg/navdata_hdvideo_stream.msg +++ b/msg/navdata_hdvideo_stream.msg @@ -1,5 +1,6 @@ Header header -float32 drone_timeuint16 tag +float64 drone_time +uint16 tag uint16 size uint32 hdvideo_state uint32 storage_fifo_nb_packets diff --git a/msg/navdata_kalman_pressure.msg b/msg/navdata_kalman_pressure.msg index e8f5dd2..81a5c7b 100644 --- a/msg/navdata_kalman_pressure.msg +++ b/msg/navdata_kalman_pressure.msg @@ -1,5 +1,6 @@ Header header -float32 drone_timeuint16 tag +float64 drone_time +uint16 tag uint16 size float32 offset_pressure float32 est_z diff --git a/msg/navdata_magneto.msg b/msg/navdata_magneto.msg index dfd62f2..b011ac6 100644 --- a/msg/navdata_magneto.msg +++ b/msg/navdata_magneto.msg @@ -1,12 +1,13 @@ Header header -float32 drone_timeuint16 tag +float64 drone_time +uint16 tag uint16 size int16 mx int16 my int16 mz -ardrone_autonomy::vector31 magneto_raw -ardrone_autonomy::vector31 magneto_rectified -ardrone_autonomy::vector31 magneto_offset +vector31 magneto_raw +vector31 magneto_rectified +vector31 magneto_offset float32 heading_unwrapped float32 heading_gyro_unwrapped float32 heading_fusion_unwrapped diff --git a/msg/navdata_phys_measures.msg b/msg/navdata_phys_measures.msg index a6b6702..832bc36 100644 --- a/msg/navdata_phys_measures.msg +++ b/msg/navdata_phys_measures.msg @@ -1,5 +1,6 @@ Header header -float32 drone_timeuint16 tag +float64 drone_time +uint16 tag uint16 size float32 accs_temp uint16 gyro_temp diff --git a/msg/navdata_pressure_raw.msg b/msg/navdata_pressure_raw.msg index cea4eae..1c672ed 100644 --- a/msg/navdata_pressure_raw.msg +++ b/msg/navdata_pressure_raw.msg @@ -1,5 +1,6 @@ Header header -float32 drone_timeuint16 tag +float64 drone_time +uint16 tag uint16 size int32 up int16 ut diff --git a/msg/navdata_pwm.msg b/msg/navdata_pwm.msg index d519843..3273a4b 100644 --- a/msg/navdata_pwm.msg +++ b/msg/navdata_pwm.msg @@ -1,5 +1,6 @@ Header header -float32 drone_timeuint16 tag +float64 drone_time +uint16 tag uint16 size uint8 motor1 uint8 motor2 @@ -25,5 +26,4 @@ uint16 current_motor1 uint16 current_motor2 uint16 current_motor3 uint16 current_motor4 -float32 altitude_prop float32 altitude_der diff --git a/msg/navdata_raw_measures.msg b/msg/navdata_raw_measures.msg index 9adfa37..1b8790c 100644 --- a/msg/navdata_raw_measures.msg +++ b/msg/navdata_raw_measures.msg @@ -1,7 +1,7 @@ Header header -float32 drone_timeuint16 tag +float64 drone_time +uint16 tag uint16 size -uint16[] raw_accs int16[] raw_gyros int16[] raw_gyros_110 uint32 vbat_raw @@ -13,7 +13,6 @@ uint16 us_courbe_temps uint16 us_courbe_valeur uint16 us_courbe_ref uint16 flag_echo_ini -uint16 frame_number uint16 nb_echo uint32 sum_echo int32 alt_temp_raw diff --git a/msg/navdata_rc_references.msg b/msg/navdata_rc_references.msg index 4e1bd37..0726087 100644 --- a/msg/navdata_rc_references.msg +++ b/msg/navdata_rc_references.msg @@ -1,5 +1,6 @@ Header header -float32 drone_timeuint16 tag +float64 drone_time +uint16 tag uint16 size int32 rc_ref_pitch int32 rc_ref_roll diff --git a/msg/navdata_references.msg b/msg/navdata_references.msg index e2103f6..c1b72e6 100644 --- a/msg/navdata_references.msg +++ b/msg/navdata_references.msg @@ -1,5 +1,6 @@ Header header -float32 drone_timeuint16 tag +float64 drone_time +uint16 tag uint16 size int32 ref_theta int32 ref_phi diff --git a/msg/navdata_time.msg b/msg/navdata_time.msg index b986c4e..52bc6a4 100644 --- a/msg/navdata_time.msg +++ b/msg/navdata_time.msg @@ -1,4 +1,5 @@ Header header -float32 drone_timeuint16 tag +float64 drone_time +uint16 tag uint16 size uint32 time diff --git a/msg/navdata_trackers_send.msg b/msg/navdata_trackers_send.msg index f3c0f1d..c401691 100644 --- a/msg/navdata_trackers_send.msg +++ b/msg/navdata_trackers_send.msg @@ -1,3 +1,6 @@ Header header -float32 drone_timeuint16 tag +float64 drone_time +uint16 tag uint16 size +int32[] locked +vector21[] point diff --git a/msg/navdata_trims.msg b/msg/navdata_trims.msg index 0ea5681..55f7ecb 100644 --- a/msg/navdata_trims.msg +++ b/msg/navdata_trims.msg @@ -1,5 +1,6 @@ Header header -float32 drone_timeuint16 tag +float64 drone_time +uint16 tag uint16 size float32 angular_rates_trim_r float32 euler_angles_trim_theta diff --git a/msg/navdata_video_stream.msg b/msg/navdata_video_stream.msg index 093a54f..d723786 100644 --- a/msg/navdata_video_stream.msg +++ b/msg/navdata_video_stream.msg @@ -1,5 +1,6 @@ Header header -float32 drone_timeuint16 tag +float64 drone_time +uint16 tag uint16 size uint8 quant uint32 frame_size @@ -8,12 +9,9 @@ uint32 atcmd_ref_seq uint32 atcmd_mean_ref_gap float32 atcmd_var_ref_gap uint32 atcmd_ref_quality -uint32 out_bitrate uint32 desired_bitrate -int32 data1 int32 data2 int32 data3 int32 data4 int32 data5 -uint32 tcp_queue_level uint32 fifo_queue_level diff --git a/msg/navdata_vision.msg b/msg/navdata_vision.msg index 05830eb..4988764 100644 --- a/msg/navdata_vision.msg +++ b/msg/navdata_vision.msg @@ -1,5 +1,6 @@ Header header -float32 drone_timeuint16 tag +float64 drone_time +uint16 tag uint16 size uint32 vision_state int32 vision_misc @@ -13,7 +14,7 @@ float32 phi_capture float32 psi_capture int32 altitude_capture uint32 time_capture -ardrone_autonomy::vector31 body_v +vector31 body_v float32 delta_phi float32 delta_theta float32 delta_psi diff --git a/msg/navdata_vision_detect.msg b/msg/navdata_vision_detect.msg index cc13cce..f0273e6 100644 --- a/msg/navdata_vision_detect.msg +++ b/msg/navdata_vision_detect.msg @@ -1,5 +1,6 @@ Header header -float32 drone_timeuint16 tag +float64 drone_time +uint16 tag uint16 size uint32 nb_detected uint32[] type @@ -9,6 +10,6 @@ uint32[] width uint32[] height uint32[] dist float32[] orientation_angle -ardrone_autonomy::matrix33[] rotation -ardrone_autonomy::vector31[] translation +matrix33[] rotation +vector31[] translation uint32[] camera_source diff --git a/msg/navdata_vision_of.msg b/msg/navdata_vision_of.msg index 910b0eb..8e2e057 100644 --- a/msg/navdata_vision_of.msg +++ b/msg/navdata_vision_of.msg @@ -1,5 +1,6 @@ Header header -float32 drone_timeuint16 tag +float64 drone_time +uint16 tag uint16 size float32[] of_dx float32[] of_dy diff --git a/msg/navdata_vision_perf.msg b/msg/navdata_vision_perf.msg index e3533d9..e2ac292 100644 --- a/msg/navdata_vision_perf.msg +++ b/msg/navdata_vision_perf.msg @@ -1,7 +1,7 @@ Header header -float32 drone_timeuint16 tag +float64 drone_time +uint16 tag uint16 size -float32 time_szo float32 time_corners float32 time_compute float32 time_tracking diff --git a/msg/navdata_vision_raw.msg b/msg/navdata_vision_raw.msg index 825061a..0860dd9 100644 --- a/msg/navdata_vision_raw.msg +++ b/msg/navdata_vision_raw.msg @@ -1,5 +1,6 @@ Header header -float32 drone_timeuint16 tag +float64 drone_time +uint16 tag uint16 size float32 vision_tx_raw float32 vision_ty_raw diff --git a/msg/navdata_watchdog.msg b/msg/navdata_watchdog.msg index 377455a..9450ed6 100644 --- a/msg/navdata_watchdog.msg +++ b/msg/navdata_watchdog.msg @@ -1,4 +1,4 @@ Header header -float32 drone_timeuint16 tag +float64 drone_time +uint16 tag uint16 size -int32 watchdog diff --git a/msg/navdata_wifi.msg b/msg/navdata_wifi.msg index 32ce0fe..5b60d4f 100644 --- a/msg/navdata_wifi.msg +++ b/msg/navdata_wifi.msg @@ -1,4 +1,5 @@ Header header -float32 drone_timeuint16 tag +float64 drone_time +uint16 tag uint16 size uint32 link_quality diff --git a/msg/navdata_wind_speed.msg b/msg/navdata_wind_speed.msg index 72861bc..f3d947f 100644 --- a/msg/navdata_wind_speed.msg +++ b/msg/navdata_wind_speed.msg @@ -1,5 +1,6 @@ Header header -float32 drone_timeuint16 tag +float64 drone_time +uint16 tag uint16 size float32 wind_speed float32 wind_angle diff --git a/msg/navdata_zimmu_3000.msg b/msg/navdata_zimmu_3000.msg index e794554..d6e8ff2 100644 --- a/msg/navdata_zimmu_3000.msg +++ b/msg/navdata_zimmu_3000.msg @@ -1,5 +1,6 @@ Header header -float32 drone_timeuint16 tag +float64 drone_time +uint16 tag uint16 size int32 vzimmuLSB float32 vzfind diff --git a/scripts/CreateNavdataFormat.py b/scripts/CreateNavdataFormat.py index 5bda82b..3686851 100644 --- a/scripts/CreateNavdataFormat.py +++ b/scripts/CreateNavdataFormat.py @@ -1,4 +1,5 @@ import re +from mako.template import Template PATH_NAVDATA_KEYS = '../ARDroneLib/Soft/Common/navdata_keys.h' PATH_NAVDATA_COMMON = '../ARDroneLib/Soft/Common/navdata_common.h' @@ -29,20 +30,24 @@ contents = '' with open(PATH_NAVDATA_COMMON,'r') as navdata_common: contents = navdata_common.read() -ros_datatypes = {'uint32_t':'uint32', - 'uint16_t':'uint16', - 'uint8_t' :'uint8', - 'int32_t':'int32', - 'int16_t':'int16', - 'int8_t' :'int8', - 'float64_t':'float64', - 'float32_t':'float32', - 'matrix33_t':'ardrone_autonomy::matrix33', - 'vector31_t':'ardrone_autonomy::vector31', - 'vector21_t':'ardrone_autonomy::vector21', - 'velocities_t':'ardrone_autonomy::vector31', - 'char':'uint8', - 'bool_t':'int32'} +ros_datatypes = {'uint32_t' :('std_msgs::UInt32','uint32_t'), + 'uint16_t' :('std_msgs::UInt16','uint16_t'), + 'uint8_t' :('std_msgs::UInt8','uint8_t'), + 'int32_t' :('std_msgs::Int32','int32_t'), + 'int16_t' :('std_msgs::Int16','int16_t'), + 'int8_t' :('std_msgs::Int8','int8_t'), + 'float64_t' :('std_msgs::Float64','float64_t'), + 'float32_t' :('std_msgs::Float32','float32_t'), + 'matrix33_t' :('ardrone_autonomy::matrix33','ardrone_autonomy::matrix33'), + 'vector31_t' :('ardrone_autonomy::vector31','ardrone_autonomy::vector31'), + 'vector21_t' :('ardrone_autonomy::vector21','ardrone_autonomy::vector21'), + 'screen_point_t' :('ardrone_autonomy::vector21','ardrone_autonomy::vector21'), + 'velocities_t' :('ardrone_autonomy::vector31','ardrone_autonomy::vector31'), + 'char' :('std_msgs::UInt8','char'), + 'bool_t' :('std_msgs::Int32','bool_t')} + + + structs = [] print 'Parsing Navdata Struct Contents' @@ -58,33 +63,50 @@ for (struct,name,_) in keys: structcontents = re.search(rg,contents) if structcontents: print '-- '+name - allmembers = re.findall(r'\s*(\w+(?:\s*?\*)?)\s*(\w+)\s*(?:\[(\w+)\])?\s*;(.*)',structcontents.groups('inside')[0]) - members = [(t,n,s) for (t,n,s,c) in allmembers if 'Deprecated' not in c] + allmembers = re.findall(r'(.*?)\s*(\w+(?:\s*?\*)?)\s*(\w+)\s*(?:\[([\w\s*/+-]+)\])?\s*;(.*)',structcontents.groups('inside')[0]) + members = [(t,n,s) for (b,t,n,s,c) in allmembers if 'Deprecated' not in c and '//' not in b] structs.append((struct,name,members)) + + + print 'Saving Custom ROS Message Definitions' for (struct,name,members) in structs: with open('../msg/'+name+'.msg','w') as f: f.write('Header header\n') - f.write('float64 drone_time') + f.write('float64 drone_time\n') for (t,n,s) in members: - dt = ros_datatypes[t] + dt = ros_datatypes[t][0] + dt = dt.rpartition('::')[2].lower() if s!='': f.write('{0}[] {1}\n'.format(dt,n)) else: f.write('{0} {1}\n'.format(dt,n)) -print 'Saving Struct Preprocessor File' + + + +print 'Generating C Source' + +items = [] +for (struct_type, struct_name, members) in structs: + item = {} + item['struct_name'] = struct_name + item['members'] = [] + for (c_type,name,size) in members: + member = {} + member['name'] = name + member['c_type'] = c_type + member['include'] = ros_datatypes[c_type][0] + member['ros_type'] = ros_datatypes[c_type][1] + member['array_size'] = size if size!='' else None + item['members'].append(member) + items.append(item) + +template = Template(filename='NavdataMessageDefinitionsTemplate.c'); + with open('../src/NavdataMessageDefinitions.h','w') as f: - for (struct,name,members) in structs: - f.write('NavdataStructStart({0},{1})\n'.format(struct,name)) - for (t,n,s) in members: - dt = ros_datatypes[t] - if s!='': - f.write('NavdataStructArray({0},{1},{2},{3},{4},{5})\n'.format(struct,name,t,dt,s,n)) - else: - f.write('NavdataStructMember({0},{1},{2},{3},{4})\n'.format(struct,name,t,dt,n)) - f.write('NavdataStructEnd({0},{1})\n\n'.format(struct,name)) + f.write(template.render(structs=items)) print 'You should now run `rosmake ardrone_autonomy` to build the custom messages.' diff --git a/scripts/NavdataMessageDefinitionsTemplate.c b/scripts/NavdataMessageDefinitionsTemplate.c new file mode 100644 index 0000000..69219a4 --- /dev/null +++ b/scripts/NavdataMessageDefinitionsTemplate.c @@ -0,0 +1,103 @@ +// Autogenerated from source-files using the CreateNavdataFormat.py script + +#ifdef NAVDATA_STRUCTS_INCLUDES +% for item in structs: +#include "ardrone_autonomy/${item['struct_name']}.h" +% endfor +<% included = [] %>\ +% for item in structs: +% for member in item['members']: +% if member['include'] not in included: +<% included.append(member['include']) %>\ +#include <${'/'.join(member['include'].split('::'))}.h> +% endif +% endfor +% endfor +#endif + +#ifdef NAVDATA_STRUCTS_HEADER +% for item in structs: + ros::Publisher pub_${item['struct_name']}; + bool enabled_${item['struct_name']}; + +% endfor + bool initialized_navdata_publishers; + void PublishNavdataTypes(navdata_unpacked_t n); +#endif + +#ifdef NAVDATA_STRUCTS_SOURCE +void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n) +{ + if(!initialized_navdata_publishers) + { + initialized_navdata_publishers = true; + +% for item in structs: + enabled_${item['struct_name']} = false; + ros::param::get("~enable_${item['struct_name']}", enabled_${item['struct_name']}); + + if(enabled_${item['struct_name']}) + { + pub_${item['struct_name']} = node_handle.advertise("ardrone/${item['struct_name']}", NAVDATA_QUEUE_SIZE); + } + +% endfor + } + +% for item in structs: + if(enabled_${item['struct_name']} && pub_${item['struct_name']}.getNumSubscribers()>0) + { + ardrone_autonomy::${item['struct_name']} msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + +% for member in item['members']: +% if member['array_size'] is None: + {\ + ${format_member(item, member, None)} + msg.${member['name']} = m; + } + +% else: + for(int i=0; i<${member['array_size']}; i++) + {\ + ${format_member(item, member, 'i')} + msg.${member['name']}.push_back(m); + } + +% endif +% endfor + pub_${item['struct_name']}.publish(msg); + } + +% endfor + +} +#endif + + + + +<%def name="format_member(item, member, i)"> + ${member['c_type']} c = n.${item['struct_name']}.${member['name']}${'[{}]'.format(i) if i is not None else ''}; + ${member['ros_type']} m; +% if 'matrix33' in member['ros_type']: + m.m11 = c.m11; + m.m12 = c.m12; + m.m13 = c.m13; + m.m21 = c.m21; + m.m22 = c.m22; + m.m23 = c.m23; + m.m31 = c.m31; + m.m32 = c.m32; + m.m33 = c.m33; +% elif 'vector21' in member['ros_type']: + m.x = c.x; + m.y = c.y; +% elif 'vector31' in member['ros_type']: + m.x = c.x; + m.y = c.y; + m.z = c.z; +% else: + m = c; +% endif + \ No newline at end of file diff --git a/src/NavdataMessageDefinitions.h b/src/NavdataMessageDefinitions.h index d3e31e6..2ed176c 100644 --- a/src/NavdataMessageDefinitions.h +++ b/src/NavdataMessageDefinitions.h @@ -1,366 +1,2846 @@ -NavdataStructStart(navdata_demo_t,navdata_demo) -NavdataStructMember(navdata_demo_t,navdata_demo,uint16_t,uint16,tag) -NavdataStructMember(navdata_demo_t,navdata_demo,uint16_t,uint16,size) -NavdataStructMember(navdata_demo_t,navdata_demo,uint32_t,uint32,ctrl_state) -NavdataStructMember(navdata_demo_t,navdata_demo,uint32_t,uint32,vbat_flying_percentage) -NavdataStructMember(navdata_demo_t,navdata_demo,float32_t,float32,theta) -NavdataStructMember(navdata_demo_t,navdata_demo,float32_t,float32,phi) -NavdataStructMember(navdata_demo_t,navdata_demo,float32_t,float32,psi) -NavdataStructMember(navdata_demo_t,navdata_demo,int32_t,int32,altitude) -NavdataStructMember(navdata_demo_t,navdata_demo,float32_t,float32,vx) -NavdataStructMember(navdata_demo_t,navdata_demo,float32_t,float32,vy) -NavdataStructMember(navdata_demo_t,navdata_demo,float32_t,float32,vz) -NavdataStructMember(navdata_demo_t,navdata_demo,uint32_t,uint32,num_frames) -NavdataStructMember(navdata_demo_t,navdata_demo,uint32_t,uint32,detection_camera_type) -NavdataStructEnd(navdata_demo_t,navdata_demo) +// Autogenerated from source-files using the CreateNavdataFormat.py script -NavdataStructStart(navdata_time_t,navdata_time) -NavdataStructMember(navdata_time_t,navdata_time,uint16_t,uint16,tag) -NavdataStructMember(navdata_time_t,navdata_time,uint16_t,uint16,size) -NavdataStructMember(navdata_time_t,navdata_time,uint32_t,uint32,time) -NavdataStructEnd(navdata_time_t,navdata_time) +#ifdef NAVDATA_STRUCTS_INCLUDES +#include "ardrone_autonomy/navdata_demo.h" +#include "ardrone_autonomy/navdata_time.h" +#include "ardrone_autonomy/navdata_raw_measures.h" +#include "ardrone_autonomy/navdata_phys_measures.h" +#include "ardrone_autonomy/navdata_gyros_offsets.h" +#include "ardrone_autonomy/navdata_euler_angles.h" +#include "ardrone_autonomy/navdata_references.h" +#include "ardrone_autonomy/navdata_trims.h" +#include "ardrone_autonomy/navdata_rc_references.h" +#include "ardrone_autonomy/navdata_pwm.h" +#include "ardrone_autonomy/navdata_altitude.h" +#include "ardrone_autonomy/navdata_vision_raw.h" +#include "ardrone_autonomy/navdata_vision_of.h" +#include "ardrone_autonomy/navdata_vision.h" +#include "ardrone_autonomy/navdata_vision_perf.h" +#include "ardrone_autonomy/navdata_trackers_send.h" +#include "ardrone_autonomy/navdata_vision_detect.h" +#include "ardrone_autonomy/navdata_watchdog.h" +#include "ardrone_autonomy/navdata_adc_data_frame.h" +#include "ardrone_autonomy/navdata_video_stream.h" +#include "ardrone_autonomy/navdata_games.h" +#include "ardrone_autonomy/navdata_pressure_raw.h" +#include "ardrone_autonomy/navdata_magneto.h" +#include "ardrone_autonomy/navdata_wind_speed.h" +#include "ardrone_autonomy/navdata_kalman_pressure.h" +#include "ardrone_autonomy/navdata_hdvideo_stream.h" +#include "ardrone_autonomy/navdata_wifi.h" +#include "ardrone_autonomy/navdata_zimmu_3000.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#endif -NavdataStructStart(navdata_raw_measures_t,navdata_raw_measures) -NavdataStructMember(navdata_raw_measures_t,navdata_raw_measures,uint16_t,uint16,tag) -NavdataStructMember(navdata_raw_measures_t,navdata_raw_measures,uint16_t,uint16,size) -NavdataStructArray(navdata_raw_measures_t,navdata_raw_measures,uint16_t,uint16,NB_ACCS,raw_accs) -NavdataStructArray(navdata_raw_measures_t,navdata_raw_measures,int16_t,int16,NB_GYROS,raw_gyros) -NavdataStructArray(navdata_raw_measures_t,navdata_raw_measures,int16_t,int16,2,raw_gyros_110) -NavdataStructMember(navdata_raw_measures_t,navdata_raw_measures,uint32_t,uint32,vbat_raw) -NavdataStructMember(navdata_raw_measures_t,navdata_raw_measures,uint16_t,uint16,us_debut_echo) -NavdataStructMember(navdata_raw_measures_t,navdata_raw_measures,uint16_t,uint16,us_fin_echo) -NavdataStructMember(navdata_raw_measures_t,navdata_raw_measures,uint16_t,uint16,us_association_echo) -NavdataStructMember(navdata_raw_measures_t,navdata_raw_measures,uint16_t,uint16,us_distance_echo) -NavdataStructMember(navdata_raw_measures_t,navdata_raw_measures,uint16_t,uint16,us_courbe_temps) -NavdataStructMember(navdata_raw_measures_t,navdata_raw_measures,uint16_t,uint16,us_courbe_valeur) -NavdataStructMember(navdata_raw_measures_t,navdata_raw_measures,uint16_t,uint16,us_courbe_ref) -NavdataStructMember(navdata_raw_measures_t,navdata_raw_measures,uint16_t,uint16,flag_echo_ini) -NavdataStructMember(navdata_raw_measures_t,navdata_raw_measures,uint16_t,uint16,frame_number) -NavdataStructMember(navdata_raw_measures_t,navdata_raw_measures,uint16_t,uint16,nb_echo) -NavdataStructMember(navdata_raw_measures_t,navdata_raw_measures,uint32_t,uint32,sum_echo) -NavdataStructMember(navdata_raw_measures_t,navdata_raw_measures,int32_t,int32,alt_temp_raw) -NavdataStructMember(navdata_raw_measures_t,navdata_raw_measures,int16_t,int16,gradient) -NavdataStructEnd(navdata_raw_measures_t,navdata_raw_measures) +#ifdef NAVDATA_STRUCTS_HEADER + ros::Publisher pub_navdata_demo; + bool enabled_navdata_demo; -NavdataStructStart(navdata_phys_measures_t,navdata_phys_measures) -NavdataStructMember(navdata_phys_measures_t,navdata_phys_measures,uint16_t,uint16,tag) -NavdataStructMember(navdata_phys_measures_t,navdata_phys_measures,uint16_t,uint16,size) -NavdataStructMember(navdata_phys_measures_t,navdata_phys_measures,float32_t,float32,accs_temp) -NavdataStructMember(navdata_phys_measures_t,navdata_phys_measures,uint16_t,uint16,gyro_temp) -NavdataStructArray(navdata_phys_measures_t,navdata_phys_measures,float32_t,float32,NB_ACCS,phys_accs) -NavdataStructArray(navdata_phys_measures_t,navdata_phys_measures,float32_t,float32,NB_GYROS,phys_gyros) -NavdataStructMember(navdata_phys_measures_t,navdata_phys_measures,uint32_t,uint32,alim3V3) -NavdataStructMember(navdata_phys_measures_t,navdata_phys_measures,uint32_t,uint32,vrefEpson) -NavdataStructMember(navdata_phys_measures_t,navdata_phys_measures,uint32_t,uint32,vrefIDG) -NavdataStructEnd(navdata_phys_measures_t,navdata_phys_measures) + ros::Publisher pub_navdata_time; + bool enabled_navdata_time; -NavdataStructStart(navdata_gyros_offsets_t,navdata_gyros_offsets) -NavdataStructMember(navdata_gyros_offsets_t,navdata_gyros_offsets,uint16_t,uint16,tag) -NavdataStructMember(navdata_gyros_offsets_t,navdata_gyros_offsets,uint16_t,uint16,size) -NavdataStructArray(navdata_gyros_offsets_t,navdata_gyros_offsets,float32_t,float32,NB_GYROS,offset_g) -NavdataStructEnd(navdata_gyros_offsets_t,navdata_gyros_offsets) + ros::Publisher pub_navdata_raw_measures; + bool enabled_navdata_raw_measures; -NavdataStructStart(navdata_euler_angles_t,navdata_euler_angles) -NavdataStructMember(navdata_euler_angles_t,navdata_euler_angles,uint16_t,uint16,tag) -NavdataStructMember(navdata_euler_angles_t,navdata_euler_angles,uint16_t,uint16,size) -NavdataStructMember(navdata_euler_angles_t,navdata_euler_angles,float32_t,float32,theta_a) -NavdataStructMember(navdata_euler_angles_t,navdata_euler_angles,float32_t,float32,phi_a) -NavdataStructEnd(navdata_euler_angles_t,navdata_euler_angles) + ros::Publisher pub_navdata_phys_measures; + bool enabled_navdata_phys_measures; -NavdataStructStart(navdata_references_t,navdata_references) -NavdataStructMember(navdata_references_t,navdata_references,uint16_t,uint16,tag) -NavdataStructMember(navdata_references_t,navdata_references,uint16_t,uint16,size) -NavdataStructMember(navdata_references_t,navdata_references,int32_t,int32,ref_theta) -NavdataStructMember(navdata_references_t,navdata_references,int32_t,int32,ref_phi) -NavdataStructMember(navdata_references_t,navdata_references,int32_t,int32,ref_theta_I) -NavdataStructMember(navdata_references_t,navdata_references,int32_t,int32,ref_phi_I) -NavdataStructMember(navdata_references_t,navdata_references,int32_t,int32,ref_pitch) -NavdataStructMember(navdata_references_t,navdata_references,int32_t,int32,ref_roll) -NavdataStructMember(navdata_references_t,navdata_references,int32_t,int32,ref_yaw) -NavdataStructMember(navdata_references_t,navdata_references,int32_t,int32,ref_psi) -NavdataStructMember(navdata_references_t,navdata_references,float32_t,float32,vx_ref) -NavdataStructMember(navdata_references_t,navdata_references,float32_t,float32,vy_ref) -NavdataStructMember(navdata_references_t,navdata_references,float32_t,float32,theta_mod) -NavdataStructMember(navdata_references_t,navdata_references,float32_t,float32,phi_mod) -NavdataStructMember(navdata_references_t,navdata_references,float32_t,float32,k_v_x) -NavdataStructMember(navdata_references_t,navdata_references,float32_t,float32,k_v_y) -NavdataStructMember(navdata_references_t,navdata_references,uint32_t,uint32,k_mode) -NavdataStructMember(navdata_references_t,navdata_references,float32_t,float32,ui_time) -NavdataStructMember(navdata_references_t,navdata_references,float32_t,float32,ui_theta) -NavdataStructMember(navdata_references_t,navdata_references,float32_t,float32,ui_phi) -NavdataStructMember(navdata_references_t,navdata_references,float32_t,float32,ui_psi) -NavdataStructMember(navdata_references_t,navdata_references,float32_t,float32,ui_psi_accuracy) -NavdataStructMember(navdata_references_t,navdata_references,int32_t,int32,ui_seq) -NavdataStructEnd(navdata_references_t,navdata_references) + ros::Publisher pub_navdata_gyros_offsets; + bool enabled_navdata_gyros_offsets; -NavdataStructStart(navdata_trims_t,navdata_trims) -NavdataStructMember(navdata_trims_t,navdata_trims,uint16_t,uint16,tag) -NavdataStructMember(navdata_trims_t,navdata_trims,uint16_t,uint16,size) -NavdataStructMember(navdata_trims_t,navdata_trims,float32_t,float32,angular_rates_trim_r) -NavdataStructMember(navdata_trims_t,navdata_trims,float32_t,float32,euler_angles_trim_theta) -NavdataStructMember(navdata_trims_t,navdata_trims,float32_t,float32,euler_angles_trim_phi) -NavdataStructEnd(navdata_trims_t,navdata_trims) + ros::Publisher pub_navdata_euler_angles; + bool enabled_navdata_euler_angles; -NavdataStructStart(navdata_rc_references_t,navdata_rc_references) -NavdataStructMember(navdata_rc_references_t,navdata_rc_references,uint16_t,uint16,tag) -NavdataStructMember(navdata_rc_references_t,navdata_rc_references,uint16_t,uint16,size) -NavdataStructMember(navdata_rc_references_t,navdata_rc_references,int32_t,int32,rc_ref_pitch) -NavdataStructMember(navdata_rc_references_t,navdata_rc_references,int32_t,int32,rc_ref_roll) -NavdataStructMember(navdata_rc_references_t,navdata_rc_references,int32_t,int32,rc_ref_yaw) -NavdataStructMember(navdata_rc_references_t,navdata_rc_references,int32_t,int32,rc_ref_gaz) -NavdataStructMember(navdata_rc_references_t,navdata_rc_references,int32_t,int32,rc_ref_ag) -NavdataStructEnd(navdata_rc_references_t,navdata_rc_references) + ros::Publisher pub_navdata_references; + bool enabled_navdata_references; -NavdataStructStart(navdata_pwm_t,navdata_pwm) -NavdataStructMember(navdata_pwm_t,navdata_pwm,uint16_t,uint16,tag) -NavdataStructMember(navdata_pwm_t,navdata_pwm,uint16_t,uint16,size) -NavdataStructMember(navdata_pwm_t,navdata_pwm,uint8_t,uint8,motor1) -NavdataStructMember(navdata_pwm_t,navdata_pwm,uint8_t,uint8,motor2) -NavdataStructMember(navdata_pwm_t,navdata_pwm,uint8_t,uint8,motor3) -NavdataStructMember(navdata_pwm_t,navdata_pwm,uint8_t,uint8,motor4) -NavdataStructMember(navdata_pwm_t,navdata_pwm,uint8_t,uint8,sat_motor1) -NavdataStructMember(navdata_pwm_t,navdata_pwm,uint8_t,uint8,sat_motor2) -NavdataStructMember(navdata_pwm_t,navdata_pwm,uint8_t,uint8,sat_motor3) -NavdataStructMember(navdata_pwm_t,navdata_pwm,uint8_t,uint8,sat_motor4) -NavdataStructMember(navdata_pwm_t,navdata_pwm,float32_t,float32,gaz_feed_forward) -NavdataStructMember(navdata_pwm_t,navdata_pwm,float32_t,float32,gaz_altitude) -NavdataStructMember(navdata_pwm_t,navdata_pwm,float32_t,float32,altitude_integral) -NavdataStructMember(navdata_pwm_t,navdata_pwm,float32_t,float32,vz_ref) -NavdataStructMember(navdata_pwm_t,navdata_pwm,int32_t,int32,u_pitch) -NavdataStructMember(navdata_pwm_t,navdata_pwm,int32_t,int32,u_roll) -NavdataStructMember(navdata_pwm_t,navdata_pwm,int32_t,int32,u_yaw) -NavdataStructMember(navdata_pwm_t,navdata_pwm,float32_t,float32,yaw_u_I) -NavdataStructMember(navdata_pwm_t,navdata_pwm,int32_t,int32,u_pitch_planif) -NavdataStructMember(navdata_pwm_t,navdata_pwm,int32_t,int32,u_roll_planif) -NavdataStructMember(navdata_pwm_t,navdata_pwm,int32_t,int32,u_yaw_planif) -NavdataStructMember(navdata_pwm_t,navdata_pwm,float32_t,float32,u_gaz_planif) -NavdataStructMember(navdata_pwm_t,navdata_pwm,uint16_t,uint16,current_motor1) -NavdataStructMember(navdata_pwm_t,navdata_pwm,uint16_t,uint16,current_motor2) -NavdataStructMember(navdata_pwm_t,navdata_pwm,uint16_t,uint16,current_motor3) -NavdataStructMember(navdata_pwm_t,navdata_pwm,uint16_t,uint16,current_motor4) -NavdataStructMember(navdata_pwm_t,navdata_pwm,float32_t,float32,altitude_prop) -NavdataStructMember(navdata_pwm_t,navdata_pwm,float32_t,float32,altitude_der) -NavdataStructEnd(navdata_pwm_t,navdata_pwm) + ros::Publisher pub_navdata_trims; + bool enabled_navdata_trims; -NavdataStructStart(navdata_altitude_t,navdata_altitude) -NavdataStructMember(navdata_altitude_t,navdata_altitude,uint16_t,uint16,tag) -NavdataStructMember(navdata_altitude_t,navdata_altitude,uint16_t,uint16,size) -NavdataStructMember(navdata_altitude_t,navdata_altitude,int32_t,int32,altitude_vision) -NavdataStructMember(navdata_altitude_t,navdata_altitude,float32_t,float32,altitude_vz) -NavdataStructMember(navdata_altitude_t,navdata_altitude,int32_t,int32,altitude_ref) -NavdataStructMember(navdata_altitude_t,navdata_altitude,int32_t,int32,altitude_raw) -NavdataStructMember(navdata_altitude_t,navdata_altitude,float32_t,float32,obs_accZ) -NavdataStructMember(navdata_altitude_t,navdata_altitude,float32_t,float32,obs_alt) -NavdataStructMember(navdata_altitude_t,navdata_altitude,vector31_t,ardrone_autonomy::vector31,obs_x) -NavdataStructMember(navdata_altitude_t,navdata_altitude,uint32_t,uint32,obs_state) -NavdataStructMember(navdata_altitude_t,navdata_altitude,vector21_t,ardrone_autonomy::vector21,est_vb) -NavdataStructMember(navdata_altitude_t,navdata_altitude,uint32_t,uint32,est_state) -NavdataStructEnd(navdata_altitude_t,navdata_altitude) + ros::Publisher pub_navdata_rc_references; + bool enabled_navdata_rc_references; -NavdataStructStart(navdata_vision_raw_t,navdata_vision_raw) -NavdataStructMember(navdata_vision_raw_t,navdata_vision_raw,uint16_t,uint16,tag) -NavdataStructMember(navdata_vision_raw_t,navdata_vision_raw,uint16_t,uint16,size) -NavdataStructMember(navdata_vision_raw_t,navdata_vision_raw,float32_t,float32,vision_tx_raw) -NavdataStructMember(navdata_vision_raw_t,navdata_vision_raw,float32_t,float32,vision_ty_raw) -NavdataStructMember(navdata_vision_raw_t,navdata_vision_raw,float32_t,float32,vision_tz_raw) -NavdataStructEnd(navdata_vision_raw_t,navdata_vision_raw) + ros::Publisher pub_navdata_pwm; + bool enabled_navdata_pwm; -NavdataStructStart(navdata_vision_of_t,navdata_vision_of) -NavdataStructMember(navdata_vision_of_t,navdata_vision_of,uint16_t,uint16,tag) -NavdataStructMember(navdata_vision_of_t,navdata_vision_of,uint16_t,uint16,size) -NavdataStructArray(navdata_vision_of_t,navdata_vision_of,float32_t,float32,5,of_dx) -NavdataStructArray(navdata_vision_of_t,navdata_vision_of,float32_t,float32,5,of_dy) -NavdataStructEnd(navdata_vision_of_t,navdata_vision_of) + ros::Publisher pub_navdata_altitude; + bool enabled_navdata_altitude; -NavdataStructStart(navdata_vision_t,navdata_vision) -NavdataStructMember(navdata_vision_t,navdata_vision,uint16_t,uint16,tag) -NavdataStructMember(navdata_vision_t,navdata_vision,uint16_t,uint16,size) -NavdataStructMember(navdata_vision_t,navdata_vision,uint32_t,uint32,vision_state) -NavdataStructMember(navdata_vision_t,navdata_vision,int32_t,int32,vision_misc) -NavdataStructMember(navdata_vision_t,navdata_vision,float32_t,float32,vision_phi_trim) -NavdataStructMember(navdata_vision_t,navdata_vision,float32_t,float32,vision_phi_ref_prop) -NavdataStructMember(navdata_vision_t,navdata_vision,float32_t,float32,vision_theta_trim) -NavdataStructMember(navdata_vision_t,navdata_vision,float32_t,float32,vision_theta_ref_prop) -NavdataStructMember(navdata_vision_t,navdata_vision,int32_t,int32,new_raw_picture) -NavdataStructMember(navdata_vision_t,navdata_vision,float32_t,float32,theta_capture) -NavdataStructMember(navdata_vision_t,navdata_vision,float32_t,float32,phi_capture) -NavdataStructMember(navdata_vision_t,navdata_vision,float32_t,float32,psi_capture) -NavdataStructMember(navdata_vision_t,navdata_vision,int32_t,int32,altitude_capture) -NavdataStructMember(navdata_vision_t,navdata_vision,uint32_t,uint32,time_capture) -NavdataStructMember(navdata_vision_t,navdata_vision,velocities_t,ardrone_autonomy::vector31,body_v) -NavdataStructMember(navdata_vision_t,navdata_vision,float32_t,float32,delta_phi) -NavdataStructMember(navdata_vision_t,navdata_vision,float32_t,float32,delta_theta) -NavdataStructMember(navdata_vision_t,navdata_vision,float32_t,float32,delta_psi) -NavdataStructMember(navdata_vision_t,navdata_vision,uint32_t,uint32,gold_defined) -NavdataStructMember(navdata_vision_t,navdata_vision,uint32_t,uint32,gold_reset) -NavdataStructMember(navdata_vision_t,navdata_vision,float32_t,float32,gold_x) -NavdataStructMember(navdata_vision_t,navdata_vision,float32_t,float32,gold_y) -NavdataStructEnd(navdata_vision_t,navdata_vision) + ros::Publisher pub_navdata_vision_raw; + bool enabled_navdata_vision_raw; -NavdataStructStart(navdata_vision_perf_t,navdata_vision_perf) -NavdataStructMember(navdata_vision_perf_t,navdata_vision_perf,uint16_t,uint16,tag) -NavdataStructMember(navdata_vision_perf_t,navdata_vision_perf,uint16_t,uint16,size) -NavdataStructMember(navdata_vision_perf_t,navdata_vision_perf,float32_t,float32,time_szo) -NavdataStructMember(navdata_vision_perf_t,navdata_vision_perf,float32_t,float32,time_corners) -NavdataStructMember(navdata_vision_perf_t,navdata_vision_perf,float32_t,float32,time_compute) -NavdataStructMember(navdata_vision_perf_t,navdata_vision_perf,float32_t,float32,time_tracking) -NavdataStructMember(navdata_vision_perf_t,navdata_vision_perf,float32_t,float32,time_trans) -NavdataStructMember(navdata_vision_perf_t,navdata_vision_perf,float32_t,float32,time_update) -NavdataStructArray(navdata_vision_perf_t,navdata_vision_perf,float32_t,float32,NAVDATA_MAX_CUSTOM_TIME_SAVE,time_custom) -NavdataStructEnd(navdata_vision_perf_t,navdata_vision_perf) + ros::Publisher pub_navdata_vision_of; + bool enabled_navdata_vision_of; -NavdataStructStart(navdata_trackers_send_t,navdata_trackers_send) -NavdataStructMember(navdata_trackers_send_t,navdata_trackers_send,uint16_t,uint16,tag) -NavdataStructMember(navdata_trackers_send_t,navdata_trackers_send,uint16_t,uint16,size) -NavdataStructEnd(navdata_trackers_send_t,navdata_trackers_send) + ros::Publisher pub_navdata_vision; + bool enabled_navdata_vision; -NavdataStructStart(navdata_vision_detect_t,navdata_vision_detect) -NavdataStructMember(navdata_vision_detect_t,navdata_vision_detect,uint16_t,uint16,tag) -NavdataStructMember(navdata_vision_detect_t,navdata_vision_detect,uint16_t,uint16,size) -NavdataStructMember(navdata_vision_detect_t,navdata_vision_detect,uint32_t,uint32,nb_detected) -NavdataStructArray(navdata_vision_detect_t,navdata_vision_detect,uint32_t,uint32,NB_NAVDATA_DETECTION_RESULTS,type) -NavdataStructArray(navdata_vision_detect_t,navdata_vision_detect,uint32_t,uint32,NB_NAVDATA_DETECTION_RESULTS,xc) -NavdataStructArray(navdata_vision_detect_t,navdata_vision_detect,uint32_t,uint32,NB_NAVDATA_DETECTION_RESULTS,yc) -NavdataStructArray(navdata_vision_detect_t,navdata_vision_detect,uint32_t,uint32,NB_NAVDATA_DETECTION_RESULTS,width) -NavdataStructArray(navdata_vision_detect_t,navdata_vision_detect,uint32_t,uint32,NB_NAVDATA_DETECTION_RESULTS,height) -NavdataStructArray(navdata_vision_detect_t,navdata_vision_detect,uint32_t,uint32,NB_NAVDATA_DETECTION_RESULTS,dist) -NavdataStructArray(navdata_vision_detect_t,navdata_vision_detect,float32_t,float32,NB_NAVDATA_DETECTION_RESULTS,orientation_angle) -NavdataStructArray(navdata_vision_detect_t,navdata_vision_detect,matrix33_t,ardrone_autonomy::matrix33,NB_NAVDATA_DETECTION_RESULTS,rotation) -NavdataStructArray(navdata_vision_detect_t,navdata_vision_detect,vector31_t,ardrone_autonomy::vector31,NB_NAVDATA_DETECTION_RESULTS,translation) -NavdataStructArray(navdata_vision_detect_t,navdata_vision_detect,uint32_t,uint32,NB_NAVDATA_DETECTION_RESULTS,camera_source) -NavdataStructEnd(navdata_vision_detect_t,navdata_vision_detect) + ros::Publisher pub_navdata_vision_perf; + bool enabled_navdata_vision_perf; -NavdataStructStart(navdata_watchdog_t,navdata_watchdog) -NavdataStructMember(navdata_watchdog_t,navdata_watchdog,uint16_t,uint16,tag) -NavdataStructMember(navdata_watchdog_t,navdata_watchdog,uint16_t,uint16,size) -NavdataStructMember(navdata_watchdog_t,navdata_watchdog,int32_t,int32,watchdog) -NavdataStructEnd(navdata_watchdog_t,navdata_watchdog) + ros::Publisher pub_navdata_trackers_send; + bool enabled_navdata_trackers_send; -NavdataStructStart(navdata_adc_data_frame_t,navdata_adc_data_frame) -NavdataStructMember(navdata_adc_data_frame_t,navdata_adc_data_frame,uint16_t,uint16,tag) -NavdataStructMember(navdata_adc_data_frame_t,navdata_adc_data_frame,uint16_t,uint16,size) -NavdataStructMember(navdata_adc_data_frame_t,navdata_adc_data_frame,uint32_t,uint32,version) -NavdataStructArray(navdata_adc_data_frame_t,navdata_adc_data_frame,uint8_t,uint8,32,data_frame) -NavdataStructEnd(navdata_adc_data_frame_t,navdata_adc_data_frame) + ros::Publisher pub_navdata_vision_detect; + bool enabled_navdata_vision_detect; -NavdataStructStart(navdata_video_stream_t,navdata_video_stream) -NavdataStructMember(navdata_video_stream_t,navdata_video_stream,uint16_t,uint16,tag) -NavdataStructMember(navdata_video_stream_t,navdata_video_stream,uint16_t,uint16,size) -NavdataStructMember(navdata_video_stream_t,navdata_video_stream,uint8_t,uint8,quant) -NavdataStructMember(navdata_video_stream_t,navdata_video_stream,uint32_t,uint32,frame_size) -NavdataStructMember(navdata_video_stream_t,navdata_video_stream,uint32_t,uint32,frame_number) -NavdataStructMember(navdata_video_stream_t,navdata_video_stream,uint32_t,uint32,atcmd_ref_seq) -NavdataStructMember(navdata_video_stream_t,navdata_video_stream,uint32_t,uint32,atcmd_mean_ref_gap) -NavdataStructMember(navdata_video_stream_t,navdata_video_stream,float32_t,float32,atcmd_var_ref_gap) -NavdataStructMember(navdata_video_stream_t,navdata_video_stream,uint32_t,uint32,atcmd_ref_quality) -NavdataStructMember(navdata_video_stream_t,navdata_video_stream,uint32_t,uint32,out_bitrate) -NavdataStructMember(navdata_video_stream_t,navdata_video_stream,uint32_t,uint32,desired_bitrate) -NavdataStructMember(navdata_video_stream_t,navdata_video_stream,int32_t,int32,data1) -NavdataStructMember(navdata_video_stream_t,navdata_video_stream,int32_t,int32,data2) -NavdataStructMember(navdata_video_stream_t,navdata_video_stream,int32_t,int32,data3) -NavdataStructMember(navdata_video_stream_t,navdata_video_stream,int32_t,int32,data4) -NavdataStructMember(navdata_video_stream_t,navdata_video_stream,int32_t,int32,data5) -NavdataStructMember(navdata_video_stream_t,navdata_video_stream,uint32_t,uint32,tcp_queue_level) -NavdataStructMember(navdata_video_stream_t,navdata_video_stream,uint32_t,uint32,fifo_queue_level) -NavdataStructEnd(navdata_video_stream_t,navdata_video_stream) + ros::Publisher pub_navdata_watchdog; + bool enabled_navdata_watchdog; -NavdataStructStart(navdata_games_t,navdata_games) -NavdataStructMember(navdata_games_t,navdata_games,uint16_t,uint16,tag) -NavdataStructMember(navdata_games_t,navdata_games,uint16_t,uint16,size) -NavdataStructMember(navdata_games_t,navdata_games,uint32_t,uint32,double_tap_counter) -NavdataStructMember(navdata_games_t,navdata_games,uint32_t,uint32,finish_line_counter) -NavdataStructEnd(navdata_games_t,navdata_games) + ros::Publisher pub_navdata_adc_data_frame; + bool enabled_navdata_adc_data_frame; -NavdataStructStart(navdata_pressure_raw_t,navdata_pressure_raw) -NavdataStructMember(navdata_pressure_raw_t,navdata_pressure_raw,uint16_t,uint16,tag) -NavdataStructMember(navdata_pressure_raw_t,navdata_pressure_raw,uint16_t,uint16,size) -NavdataStructMember(navdata_pressure_raw_t,navdata_pressure_raw,int32_t,int32,up) -NavdataStructMember(navdata_pressure_raw_t,navdata_pressure_raw,int16_t,int16,ut) -NavdataStructMember(navdata_pressure_raw_t,navdata_pressure_raw,int32_t,int32,Temperature_meas) -NavdataStructMember(navdata_pressure_raw_t,navdata_pressure_raw,int32_t,int32,Pression_meas) -NavdataStructEnd(navdata_pressure_raw_t,navdata_pressure_raw) + ros::Publisher pub_navdata_video_stream; + bool enabled_navdata_video_stream; -NavdataStructStart(navdata_magneto_t,navdata_magneto) -NavdataStructMember(navdata_magneto_t,navdata_magneto,uint16_t,uint16,tag) -NavdataStructMember(navdata_magneto_t,navdata_magneto,uint16_t,uint16,size) -NavdataStructMember(navdata_magneto_t,navdata_magneto,int16_t,int16,mx) -NavdataStructMember(navdata_magneto_t,navdata_magneto,int16_t,int16,my) -NavdataStructMember(navdata_magneto_t,navdata_magneto,int16_t,int16,mz) -NavdataStructMember(navdata_magneto_t,navdata_magneto,vector31_t,ardrone_autonomy::vector31,magneto_raw) -NavdataStructMember(navdata_magneto_t,navdata_magneto,vector31_t,ardrone_autonomy::vector31,magneto_rectified) -NavdataStructMember(navdata_magneto_t,navdata_magneto,vector31_t,ardrone_autonomy::vector31,magneto_offset) -NavdataStructMember(navdata_magneto_t,navdata_magneto,float32_t,float32,heading_unwrapped) -NavdataStructMember(navdata_magneto_t,navdata_magneto,float32_t,float32,heading_gyro_unwrapped) -NavdataStructMember(navdata_magneto_t,navdata_magneto,float32_t,float32,heading_fusion_unwrapped) -NavdataStructMember(navdata_magneto_t,navdata_magneto,char,uint8,magneto_calibration_ok) -NavdataStructMember(navdata_magneto_t,navdata_magneto,uint32_t,uint32,magneto_state) -NavdataStructMember(navdata_magneto_t,navdata_magneto,float32_t,float32,magneto_radius) -NavdataStructMember(navdata_magneto_t,navdata_magneto,float32_t,float32,error_mean) -NavdataStructMember(navdata_magneto_t,navdata_magneto,float32_t,float32,error_var) -NavdataStructEnd(navdata_magneto_t,navdata_magneto) + ros::Publisher pub_navdata_games; + bool enabled_navdata_games; -NavdataStructStart(navdata_wind_speed_t,navdata_wind_speed) -NavdataStructMember(navdata_wind_speed_t,navdata_wind_speed,uint16_t,uint16,tag) -NavdataStructMember(navdata_wind_speed_t,navdata_wind_speed,uint16_t,uint16,size) -NavdataStructMember(navdata_wind_speed_t,navdata_wind_speed,float32_t,float32,wind_speed) -NavdataStructMember(navdata_wind_speed_t,navdata_wind_speed,float32_t,float32,wind_angle) -NavdataStructMember(navdata_wind_speed_t,navdata_wind_speed,float32_t,float32,wind_compensation_theta) -NavdataStructMember(navdata_wind_speed_t,navdata_wind_speed,float32_t,float32,wind_compensation_phi) -NavdataStructMember(navdata_wind_speed_t,navdata_wind_speed,float32_t,float32,state_x1) -NavdataStructMember(navdata_wind_speed_t,navdata_wind_speed,float32_t,float32,state_x2) -NavdataStructMember(navdata_wind_speed_t,navdata_wind_speed,float32_t,float32,state_x3) -NavdataStructMember(navdata_wind_speed_t,navdata_wind_speed,float32_t,float32,state_x4) -NavdataStructMember(navdata_wind_speed_t,navdata_wind_speed,float32_t,float32,state_x5) -NavdataStructMember(navdata_wind_speed_t,navdata_wind_speed,float32_t,float32,state_x6) -NavdataStructMember(navdata_wind_speed_t,navdata_wind_speed,float32_t,float32,magneto_debug1) -NavdataStructMember(navdata_wind_speed_t,navdata_wind_speed,float32_t,float32,magneto_debug2) -NavdataStructMember(navdata_wind_speed_t,navdata_wind_speed,float32_t,float32,magneto_debug3) -NavdataStructEnd(navdata_wind_speed_t,navdata_wind_speed) + ros::Publisher pub_navdata_pressure_raw; + bool enabled_navdata_pressure_raw; -NavdataStructStart(navdata_kalman_pressure_t,navdata_kalman_pressure) -NavdataStructMember(navdata_kalman_pressure_t,navdata_kalman_pressure,uint16_t,uint16,tag) -NavdataStructMember(navdata_kalman_pressure_t,navdata_kalman_pressure,uint16_t,uint16,size) -NavdataStructMember(navdata_kalman_pressure_t,navdata_kalman_pressure,float32_t,float32,offset_pressure) -NavdataStructMember(navdata_kalman_pressure_t,navdata_kalman_pressure,float32_t,float32,est_z) -NavdataStructMember(navdata_kalman_pressure_t,navdata_kalman_pressure,float32_t,float32,est_zdot) -NavdataStructMember(navdata_kalman_pressure_t,navdata_kalman_pressure,float32_t,float32,est_bias_PWM) -NavdataStructMember(navdata_kalman_pressure_t,navdata_kalman_pressure,float32_t,float32,est_biais_pression) -NavdataStructMember(navdata_kalman_pressure_t,navdata_kalman_pressure,float32_t,float32,offset_US) -NavdataStructMember(navdata_kalman_pressure_t,navdata_kalman_pressure,float32_t,float32,prediction_US) -NavdataStructMember(navdata_kalman_pressure_t,navdata_kalman_pressure,float32_t,float32,cov_alt) -NavdataStructMember(navdata_kalman_pressure_t,navdata_kalman_pressure,float32_t,float32,cov_PWM) -NavdataStructMember(navdata_kalman_pressure_t,navdata_kalman_pressure,float32_t,float32,cov_vitesse) -NavdataStructMember(navdata_kalman_pressure_t,navdata_kalman_pressure,bool_t,int32,bool_effet_sol) -NavdataStructMember(navdata_kalman_pressure_t,navdata_kalman_pressure,float32_t,float32,somme_inno) -NavdataStructMember(navdata_kalman_pressure_t,navdata_kalman_pressure,bool_t,int32,flag_rejet_US) -NavdataStructMember(navdata_kalman_pressure_t,navdata_kalman_pressure,float32_t,float32,u_multisinus) -NavdataStructMember(navdata_kalman_pressure_t,navdata_kalman_pressure,float32_t,float32,gaz_altitude) -NavdataStructMember(navdata_kalman_pressure_t,navdata_kalman_pressure,bool_t,int32,Flag_multisinus) -NavdataStructMember(navdata_kalman_pressure_t,navdata_kalman_pressure,bool_t,int32,Flag_multisinus_debut) -NavdataStructEnd(navdata_kalman_pressure_t,navdata_kalman_pressure) + ros::Publisher pub_navdata_magneto; + bool enabled_navdata_magneto; + + ros::Publisher pub_navdata_wind_speed; + bool enabled_navdata_wind_speed; + + ros::Publisher pub_navdata_kalman_pressure; + bool enabled_navdata_kalman_pressure; + + ros::Publisher pub_navdata_hdvideo_stream; + bool enabled_navdata_hdvideo_stream; + + ros::Publisher pub_navdata_wifi; + bool enabled_navdata_wifi; + + ros::Publisher pub_navdata_zimmu_3000; + bool enabled_navdata_zimmu_3000; + + bool initialized_navdata_publishers; + void PublishNavdataTypes(navdata_unpacked_t n); +#endif + +#ifdef NAVDATA_STRUCTS_SOURCE +void ARDroneDriver::PublishNavdataTypes(navdata_unpacked_t n) +{ + if(!initialized_navdata_publishers) + { + initialized_navdata_publishers = true; + + enabled_navdata_demo = false; + ros::param::get("~enable_navdata_demo", enabled_navdata_demo); + + if(enabled_navdata_demo) + { + pub_navdata_demo = node_handle.advertise("ardrone/navdata_demo", NAVDATA_QUEUE_SIZE); + } + + enabled_navdata_time = false; + ros::param::get("~enable_navdata_time", enabled_navdata_time); + + if(enabled_navdata_time) + { + pub_navdata_time = node_handle.advertise("ardrone/navdata_time", NAVDATA_QUEUE_SIZE); + } + + enabled_navdata_raw_measures = false; + ros::param::get("~enable_navdata_raw_measures", enabled_navdata_raw_measures); + + if(enabled_navdata_raw_measures) + { + pub_navdata_raw_measures = node_handle.advertise("ardrone/navdata_raw_measures", NAVDATA_QUEUE_SIZE); + } + + enabled_navdata_phys_measures = false; + ros::param::get("~enable_navdata_phys_measures", enabled_navdata_phys_measures); + + if(enabled_navdata_phys_measures) + { + pub_navdata_phys_measures = node_handle.advertise("ardrone/navdata_phys_measures", NAVDATA_QUEUE_SIZE); + } + + enabled_navdata_gyros_offsets = false; + ros::param::get("~enable_navdata_gyros_offsets", enabled_navdata_gyros_offsets); + + if(enabled_navdata_gyros_offsets) + { + pub_navdata_gyros_offsets = node_handle.advertise("ardrone/navdata_gyros_offsets", NAVDATA_QUEUE_SIZE); + } + + enabled_navdata_euler_angles = false; + ros::param::get("~enable_navdata_euler_angles", enabled_navdata_euler_angles); + + if(enabled_navdata_euler_angles) + { + pub_navdata_euler_angles = node_handle.advertise("ardrone/navdata_euler_angles", NAVDATA_QUEUE_SIZE); + } + + enabled_navdata_references = false; + ros::param::get("~enable_navdata_references", enabled_navdata_references); + + if(enabled_navdata_references) + { + pub_navdata_references = node_handle.advertise("ardrone/navdata_references", NAVDATA_QUEUE_SIZE); + } + + enabled_navdata_trims = false; + ros::param::get("~enable_navdata_trims", enabled_navdata_trims); + + if(enabled_navdata_trims) + { + pub_navdata_trims = node_handle.advertise("ardrone/navdata_trims", NAVDATA_QUEUE_SIZE); + } + + enabled_navdata_rc_references = false; + ros::param::get("~enable_navdata_rc_references", enabled_navdata_rc_references); + + if(enabled_navdata_rc_references) + { + pub_navdata_rc_references = node_handle.advertise("ardrone/navdata_rc_references", NAVDATA_QUEUE_SIZE); + } + + enabled_navdata_pwm = false; + ros::param::get("~enable_navdata_pwm", enabled_navdata_pwm); + + if(enabled_navdata_pwm) + { + pub_navdata_pwm = node_handle.advertise("ardrone/navdata_pwm", NAVDATA_QUEUE_SIZE); + } + + enabled_navdata_altitude = false; + ros::param::get("~enable_navdata_altitude", enabled_navdata_altitude); + + if(enabled_navdata_altitude) + { + pub_navdata_altitude = node_handle.advertise("ardrone/navdata_altitude", NAVDATA_QUEUE_SIZE); + } + + enabled_navdata_vision_raw = false; + ros::param::get("~enable_navdata_vision_raw", enabled_navdata_vision_raw); + + if(enabled_navdata_vision_raw) + { + pub_navdata_vision_raw = node_handle.advertise("ardrone/navdata_vision_raw", NAVDATA_QUEUE_SIZE); + } + + enabled_navdata_vision_of = false; + ros::param::get("~enable_navdata_vision_of", enabled_navdata_vision_of); + + if(enabled_navdata_vision_of) + { + pub_navdata_vision_of = node_handle.advertise("ardrone/navdata_vision_of", NAVDATA_QUEUE_SIZE); + } + + enabled_navdata_vision = false; + ros::param::get("~enable_navdata_vision", enabled_navdata_vision); + + if(enabled_navdata_vision) + { + pub_navdata_vision = node_handle.advertise("ardrone/navdata_vision", NAVDATA_QUEUE_SIZE); + } + + enabled_navdata_vision_perf = false; + ros::param::get("~enable_navdata_vision_perf", enabled_navdata_vision_perf); + + if(enabled_navdata_vision_perf) + { + pub_navdata_vision_perf = node_handle.advertise("ardrone/navdata_vision_perf", NAVDATA_QUEUE_SIZE); + } + + enabled_navdata_trackers_send = false; + ros::param::get("~enable_navdata_trackers_send", enabled_navdata_trackers_send); + + if(enabled_navdata_trackers_send) + { + pub_navdata_trackers_send = node_handle.advertise("ardrone/navdata_trackers_send", NAVDATA_QUEUE_SIZE); + } + + enabled_navdata_vision_detect = false; + ros::param::get("~enable_navdata_vision_detect", enabled_navdata_vision_detect); + + if(enabled_navdata_vision_detect) + { + pub_navdata_vision_detect = node_handle.advertise("ardrone/navdata_vision_detect", NAVDATA_QUEUE_SIZE); + } + + enabled_navdata_watchdog = false; + ros::param::get("~enable_navdata_watchdog", enabled_navdata_watchdog); + + if(enabled_navdata_watchdog) + { + pub_navdata_watchdog = node_handle.advertise("ardrone/navdata_watchdog", NAVDATA_QUEUE_SIZE); + } + + enabled_navdata_adc_data_frame = false; + ros::param::get("~enable_navdata_adc_data_frame", enabled_navdata_adc_data_frame); + + if(enabled_navdata_adc_data_frame) + { + pub_navdata_adc_data_frame = node_handle.advertise("ardrone/navdata_adc_data_frame", NAVDATA_QUEUE_SIZE); + } + + enabled_navdata_video_stream = false; + ros::param::get("~enable_navdata_video_stream", enabled_navdata_video_stream); + + if(enabled_navdata_video_stream) + { + pub_navdata_video_stream = node_handle.advertise("ardrone/navdata_video_stream", NAVDATA_QUEUE_SIZE); + } + + enabled_navdata_games = false; + ros::param::get("~enable_navdata_games", enabled_navdata_games); + + if(enabled_navdata_games) + { + pub_navdata_games = node_handle.advertise("ardrone/navdata_games", NAVDATA_QUEUE_SIZE); + } + + enabled_navdata_pressure_raw = false; + ros::param::get("~enable_navdata_pressure_raw", enabled_navdata_pressure_raw); + + if(enabled_navdata_pressure_raw) + { + pub_navdata_pressure_raw = node_handle.advertise("ardrone/navdata_pressure_raw", NAVDATA_QUEUE_SIZE); + } + + enabled_navdata_magneto = false; + ros::param::get("~enable_navdata_magneto", enabled_navdata_magneto); + + if(enabled_navdata_magneto) + { + pub_navdata_magneto = node_handle.advertise("ardrone/navdata_magneto", NAVDATA_QUEUE_SIZE); + } + + enabled_navdata_wind_speed = false; + ros::param::get("~enable_navdata_wind_speed", enabled_navdata_wind_speed); + + if(enabled_navdata_wind_speed) + { + pub_navdata_wind_speed = node_handle.advertise("ardrone/navdata_wind_speed", NAVDATA_QUEUE_SIZE); + } + + enabled_navdata_kalman_pressure = false; + ros::param::get("~enable_navdata_kalman_pressure", enabled_navdata_kalman_pressure); + + if(enabled_navdata_kalman_pressure) + { + pub_navdata_kalman_pressure = node_handle.advertise("ardrone/navdata_kalman_pressure", NAVDATA_QUEUE_SIZE); + } + + enabled_navdata_hdvideo_stream = false; + ros::param::get("~enable_navdata_hdvideo_stream", enabled_navdata_hdvideo_stream); + + if(enabled_navdata_hdvideo_stream) + { + pub_navdata_hdvideo_stream = node_handle.advertise("ardrone/navdata_hdvideo_stream", NAVDATA_QUEUE_SIZE); + } + + enabled_navdata_wifi = false; + ros::param::get("~enable_navdata_wifi", enabled_navdata_wifi); + + if(enabled_navdata_wifi) + { + pub_navdata_wifi = node_handle.advertise("ardrone/navdata_wifi", NAVDATA_QUEUE_SIZE); + } + + enabled_navdata_zimmu_3000 = false; + ros::param::get("~enable_navdata_zimmu_3000", enabled_navdata_zimmu_3000); + + if(enabled_navdata_zimmu_3000) + { + pub_navdata_zimmu_3000 = node_handle.advertise("ardrone/navdata_zimmu_3000", NAVDATA_QUEUE_SIZE); + } + + } + + if(enabled_navdata_demo && pub_navdata_demo.getNumSubscribers()>0) + { + ardrone_autonomy::navdata_demo msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + + { + uint16_t c = n.navdata_demo.tag; + uint16_t m; + m = c; + + msg.tag = m; + } + + { + uint16_t c = n.navdata_demo.size; + uint16_t m; + m = c; + + msg.size = m; + } + + { + uint32_t c = n.navdata_demo.ctrl_state; + uint32_t m; + m = c; + + msg.ctrl_state = m; + } + + { + uint32_t c = n.navdata_demo.vbat_flying_percentage; + uint32_t m; + m = c; + + msg.vbat_flying_percentage = m; + } + + { + float32_t c = n.navdata_demo.theta; + float32_t m; + m = c; + + msg.theta = m; + } + + { + float32_t c = n.navdata_demo.phi; + float32_t m; + m = c; + + msg.phi = m; + } + + { + float32_t c = n.navdata_demo.psi; + float32_t m; + m = c; + + msg.psi = m; + } + + { + int32_t c = n.navdata_demo.altitude; + int32_t m; + m = c; + + msg.altitude = m; + } + + { + float32_t c = n.navdata_demo.vx; + float32_t m; + m = c; + + msg.vx = m; + } + + { + float32_t c = n.navdata_demo.vy; + float32_t m; + m = c; + + msg.vy = m; + } + + { + float32_t c = n.navdata_demo.vz; + float32_t m; + m = c; + + msg.vz = m; + } + + { + uint32_t c = n.navdata_demo.num_frames; + uint32_t m; + m = c; + + msg.num_frames = m; + } + + { + uint32_t c = n.navdata_demo.detection_camera_type; + uint32_t m; + m = c; + + msg.detection_camera_type = m; + } + + pub_navdata_demo.publish(msg); + } + + if(enabled_navdata_time && pub_navdata_time.getNumSubscribers()>0) + { + ardrone_autonomy::navdata_time msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + + { + uint16_t c = n.navdata_time.tag; + uint16_t m; + m = c; + + msg.tag = m; + } + + { + uint16_t c = n.navdata_time.size; + uint16_t m; + m = c; + + msg.size = m; + } + + { + uint32_t c = n.navdata_time.time; + uint32_t m; + m = c; + + msg.time = m; + } + + pub_navdata_time.publish(msg); + } + + if(enabled_navdata_raw_measures && pub_navdata_raw_measures.getNumSubscribers()>0) + { + ardrone_autonomy::navdata_raw_measures msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + + { + uint16_t c = n.navdata_raw_measures.tag; + uint16_t m; + m = c; + + msg.tag = m; + } + + { + uint16_t c = n.navdata_raw_measures.size; + uint16_t m; + m = c; + + msg.size = m; + } + + for(int i=0; i0) + { + ardrone_autonomy::navdata_phys_measures msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + + { + uint16_t c = n.navdata_phys_measures.tag; + uint16_t m; + m = c; + + msg.tag = m; + } + + { + uint16_t c = n.navdata_phys_measures.size; + uint16_t m; + m = c; + + msg.size = m; + } + + { + float32_t c = n.navdata_phys_measures.accs_temp; + float32_t m; + m = c; + + msg.accs_temp = m; + } + + { + uint16_t c = n.navdata_phys_measures.gyro_temp; + uint16_t m; + m = c; + + msg.gyro_temp = m; + } + + for(int i=0; i0) + { + ardrone_autonomy::navdata_gyros_offsets msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + + { + uint16_t c = n.navdata_gyros_offsets.tag; + uint16_t m; + m = c; + + msg.tag = m; + } + + { + uint16_t c = n.navdata_gyros_offsets.size; + uint16_t m; + m = c; + + msg.size = m; + } + + for(int i=0; i0) + { + ardrone_autonomy::navdata_euler_angles msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + + { + uint16_t c = n.navdata_euler_angles.tag; + uint16_t m; + m = c; + + msg.tag = m; + } + + { + uint16_t c = n.navdata_euler_angles.size; + uint16_t m; + m = c; + + msg.size = m; + } + + { + float32_t c = n.navdata_euler_angles.theta_a; + float32_t m; + m = c; + + msg.theta_a = m; + } + + { + float32_t c = n.navdata_euler_angles.phi_a; + float32_t m; + m = c; + + msg.phi_a = m; + } + + pub_navdata_euler_angles.publish(msg); + } + + if(enabled_navdata_references && pub_navdata_references.getNumSubscribers()>0) + { + ardrone_autonomy::navdata_references msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + + { + uint16_t c = n.navdata_references.tag; + uint16_t m; + m = c; + + msg.tag = m; + } + + { + uint16_t c = n.navdata_references.size; + uint16_t m; + m = c; + + msg.size = m; + } + + { + int32_t c = n.navdata_references.ref_theta; + int32_t m; + m = c; + + msg.ref_theta = m; + } + + { + int32_t c = n.navdata_references.ref_phi; + int32_t m; + m = c; + + msg.ref_phi = m; + } + + { + int32_t c = n.navdata_references.ref_theta_I; + int32_t m; + m = c; + + msg.ref_theta_I = m; + } + + { + int32_t c = n.navdata_references.ref_phi_I; + int32_t m; + m = c; + + msg.ref_phi_I = m; + } + + { + int32_t c = n.navdata_references.ref_pitch; + int32_t m; + m = c; + + msg.ref_pitch = m; + } + + { + int32_t c = n.navdata_references.ref_roll; + int32_t m; + m = c; + + msg.ref_roll = m; + } + + { + int32_t c = n.navdata_references.ref_yaw; + int32_t m; + m = c; + + msg.ref_yaw = m; + } + + { + int32_t c = n.navdata_references.ref_psi; + int32_t m; + m = c; + + msg.ref_psi = m; + } + + { + float32_t c = n.navdata_references.vx_ref; + float32_t m; + m = c; + + msg.vx_ref = m; + } + + { + float32_t c = n.navdata_references.vy_ref; + float32_t m; + m = c; + + msg.vy_ref = m; + } + + { + float32_t c = n.navdata_references.theta_mod; + float32_t m; + m = c; + + msg.theta_mod = m; + } + + { + float32_t c = n.navdata_references.phi_mod; + float32_t m; + m = c; + + msg.phi_mod = m; + } + + { + float32_t c = n.navdata_references.k_v_x; + float32_t m; + m = c; + + msg.k_v_x = m; + } + + { + float32_t c = n.navdata_references.k_v_y; + float32_t m; + m = c; + + msg.k_v_y = m; + } + + { + uint32_t c = n.navdata_references.k_mode; + uint32_t m; + m = c; + + msg.k_mode = m; + } + + { + float32_t c = n.navdata_references.ui_time; + float32_t m; + m = c; + + msg.ui_time = m; + } + + { + float32_t c = n.navdata_references.ui_theta; + float32_t m; + m = c; + + msg.ui_theta = m; + } + + { + float32_t c = n.navdata_references.ui_phi; + float32_t m; + m = c; + + msg.ui_phi = m; + } + + { + float32_t c = n.navdata_references.ui_psi; + float32_t m; + m = c; + + msg.ui_psi = m; + } + + { + float32_t c = n.navdata_references.ui_psi_accuracy; + float32_t m; + m = c; + + msg.ui_psi_accuracy = m; + } + + { + int32_t c = n.navdata_references.ui_seq; + int32_t m; + m = c; + + msg.ui_seq = m; + } + + pub_navdata_references.publish(msg); + } + + if(enabled_navdata_trims && pub_navdata_trims.getNumSubscribers()>0) + { + ardrone_autonomy::navdata_trims msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + + { + uint16_t c = n.navdata_trims.tag; + uint16_t m; + m = c; + + msg.tag = m; + } + + { + uint16_t c = n.navdata_trims.size; + uint16_t m; + m = c; + + msg.size = m; + } + + { + float32_t c = n.navdata_trims.angular_rates_trim_r; + float32_t m; + m = c; + + msg.angular_rates_trim_r = m; + } + + { + float32_t c = n.navdata_trims.euler_angles_trim_theta; + float32_t m; + m = c; + + msg.euler_angles_trim_theta = m; + } + + { + float32_t c = n.navdata_trims.euler_angles_trim_phi; + float32_t m; + m = c; + + msg.euler_angles_trim_phi = m; + } + + pub_navdata_trims.publish(msg); + } + + if(enabled_navdata_rc_references && pub_navdata_rc_references.getNumSubscribers()>0) + { + ardrone_autonomy::navdata_rc_references msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + + { + uint16_t c = n.navdata_rc_references.tag; + uint16_t m; + m = c; + + msg.tag = m; + } + + { + uint16_t c = n.navdata_rc_references.size; + uint16_t m; + m = c; + + msg.size = m; + } + + { + int32_t c = n.navdata_rc_references.rc_ref_pitch; + int32_t m; + m = c; + + msg.rc_ref_pitch = m; + } + + { + int32_t c = n.navdata_rc_references.rc_ref_roll; + int32_t m; + m = c; + + msg.rc_ref_roll = m; + } + + { + int32_t c = n.navdata_rc_references.rc_ref_yaw; + int32_t m; + m = c; + + msg.rc_ref_yaw = m; + } + + { + int32_t c = n.navdata_rc_references.rc_ref_gaz; + int32_t m; + m = c; + + msg.rc_ref_gaz = m; + } + + { + int32_t c = n.navdata_rc_references.rc_ref_ag; + int32_t m; + m = c; + + msg.rc_ref_ag = m; + } + + pub_navdata_rc_references.publish(msg); + } + + if(enabled_navdata_pwm && pub_navdata_pwm.getNumSubscribers()>0) + { + ardrone_autonomy::navdata_pwm msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + + { + uint16_t c = n.navdata_pwm.tag; + uint16_t m; + m = c; + + msg.tag = m; + } + + { + uint16_t c = n.navdata_pwm.size; + uint16_t m; + m = c; + + msg.size = m; + } + + { + uint8_t c = n.navdata_pwm.motor1; + uint8_t m; + m = c; + + msg.motor1 = m; + } + + { + uint8_t c = n.navdata_pwm.motor2; + uint8_t m; + m = c; + + msg.motor2 = m; + } + + { + uint8_t c = n.navdata_pwm.motor3; + uint8_t m; + m = c; + + msg.motor3 = m; + } + + { + uint8_t c = n.navdata_pwm.motor4; + uint8_t m; + m = c; + + msg.motor4 = m; + } + + { + uint8_t c = n.navdata_pwm.sat_motor1; + uint8_t m; + m = c; + + msg.sat_motor1 = m; + } + + { + uint8_t c = n.navdata_pwm.sat_motor2; + uint8_t m; + m = c; + + msg.sat_motor2 = m; + } + + { + uint8_t c = n.navdata_pwm.sat_motor3; + uint8_t m; + m = c; + + msg.sat_motor3 = m; + } + + { + uint8_t c = n.navdata_pwm.sat_motor4; + uint8_t m; + m = c; + + msg.sat_motor4 = m; + } + + { + float32_t c = n.navdata_pwm.gaz_feed_forward; + float32_t m; + m = c; + + msg.gaz_feed_forward = m; + } + + { + float32_t c = n.navdata_pwm.gaz_altitude; + float32_t m; + m = c; + + msg.gaz_altitude = m; + } + + { + float32_t c = n.navdata_pwm.altitude_integral; + float32_t m; + m = c; + + msg.altitude_integral = m; + } + + { + float32_t c = n.navdata_pwm.vz_ref; + float32_t m; + m = c; + + msg.vz_ref = m; + } + + { + int32_t c = n.navdata_pwm.u_pitch; + int32_t m; + m = c; + + msg.u_pitch = m; + } + + { + int32_t c = n.navdata_pwm.u_roll; + int32_t m; + m = c; + + msg.u_roll = m; + } + + { + int32_t c = n.navdata_pwm.u_yaw; + int32_t m; + m = c; + + msg.u_yaw = m; + } + + { + float32_t c = n.navdata_pwm.yaw_u_I; + float32_t m; + m = c; + + msg.yaw_u_I = m; + } + + { + int32_t c = n.navdata_pwm.u_pitch_planif; + int32_t m; + m = c; + + msg.u_pitch_planif = m; + } + + { + int32_t c = n.navdata_pwm.u_roll_planif; + int32_t m; + m = c; + + msg.u_roll_planif = m; + } + + { + int32_t c = n.navdata_pwm.u_yaw_planif; + int32_t m; + m = c; + + msg.u_yaw_planif = m; + } + + { + float32_t c = n.navdata_pwm.u_gaz_planif; + float32_t m; + m = c; + + msg.u_gaz_planif = m; + } + + { + uint16_t c = n.navdata_pwm.current_motor1; + uint16_t m; + m = c; + + msg.current_motor1 = m; + } + + { + uint16_t c = n.navdata_pwm.current_motor2; + uint16_t m; + m = c; + + msg.current_motor2 = m; + } + + { + uint16_t c = n.navdata_pwm.current_motor3; + uint16_t m; + m = c; + + msg.current_motor3 = m; + } + + { + uint16_t c = n.navdata_pwm.current_motor4; + uint16_t m; + m = c; + + msg.current_motor4 = m; + } + + { + float32_t c = n.navdata_pwm.altitude_der; + float32_t m; + m = c; + + msg.altitude_der = m; + } + + pub_navdata_pwm.publish(msg); + } + + if(enabled_navdata_altitude && pub_navdata_altitude.getNumSubscribers()>0) + { + ardrone_autonomy::navdata_altitude msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + + { + uint16_t c = n.navdata_altitude.tag; + uint16_t m; + m = c; + + msg.tag = m; + } + + { + uint16_t c = n.navdata_altitude.size; + uint16_t m; + m = c; + + msg.size = m; + } + + { + int32_t c = n.navdata_altitude.altitude_vision; + int32_t m; + m = c; + + msg.altitude_vision = m; + } + + { + float32_t c = n.navdata_altitude.altitude_vz; + float32_t m; + m = c; + + msg.altitude_vz = m; + } + + { + int32_t c = n.navdata_altitude.altitude_ref; + int32_t m; + m = c; + + msg.altitude_ref = m; + } + + { + int32_t c = n.navdata_altitude.altitude_raw; + int32_t m; + m = c; + + msg.altitude_raw = m; + } + + { + float32_t c = n.navdata_altitude.obs_accZ; + float32_t m; + m = c; + + msg.obs_accZ = m; + } + + { + float32_t c = n.navdata_altitude.obs_alt; + float32_t m; + m = c; + + msg.obs_alt = m; + } + + { + vector31_t c = n.navdata_altitude.obs_x; + ardrone_autonomy::vector31 m; + m.x = c.x; + m.y = c.y; + m.z = c.z; + + msg.obs_x = m; + } + + { + uint32_t c = n.navdata_altitude.obs_state; + uint32_t m; + m = c; + + msg.obs_state = m; + } + + { + vector21_t c = n.navdata_altitude.est_vb; + ardrone_autonomy::vector21 m; + m.x = c.x; + m.y = c.y; + + msg.est_vb = m; + } + + { + uint32_t c = n.navdata_altitude.est_state; + uint32_t m; + m = c; + + msg.est_state = m; + } + + pub_navdata_altitude.publish(msg); + } + + if(enabled_navdata_vision_raw && pub_navdata_vision_raw.getNumSubscribers()>0) + { + ardrone_autonomy::navdata_vision_raw msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + + { + uint16_t c = n.navdata_vision_raw.tag; + uint16_t m; + m = c; + + msg.tag = m; + } + + { + uint16_t c = n.navdata_vision_raw.size; + uint16_t m; + m = c; + + msg.size = m; + } + + { + float32_t c = n.navdata_vision_raw.vision_tx_raw; + float32_t m; + m = c; + + msg.vision_tx_raw = m; + } + + { + float32_t c = n.navdata_vision_raw.vision_ty_raw; + float32_t m; + m = c; + + msg.vision_ty_raw = m; + } + + { + float32_t c = n.navdata_vision_raw.vision_tz_raw; + float32_t m; + m = c; + + msg.vision_tz_raw = m; + } + + pub_navdata_vision_raw.publish(msg); + } + + if(enabled_navdata_vision_of && pub_navdata_vision_of.getNumSubscribers()>0) + { + ardrone_autonomy::navdata_vision_of msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + + { + uint16_t c = n.navdata_vision_of.tag; + uint16_t m; + m = c; + + msg.tag = m; + } + + { + uint16_t c = n.navdata_vision_of.size; + uint16_t m; + m = c; + + msg.size = m; + } + + for(int i=0; i<5; i++) + { + float32_t c = n.navdata_vision_of.of_dx[i]; + float32_t m; + m = c; + + msg.of_dx.push_back(m); + } + + for(int i=0; i<5; i++) + { + float32_t c = n.navdata_vision_of.of_dy[i]; + float32_t m; + m = c; + + msg.of_dy.push_back(m); + } + + pub_navdata_vision_of.publish(msg); + } + + if(enabled_navdata_vision && pub_navdata_vision.getNumSubscribers()>0) + { + ardrone_autonomy::navdata_vision msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + + { + uint16_t c = n.navdata_vision.tag; + uint16_t m; + m = c; + + msg.tag = m; + } + + { + uint16_t c = n.navdata_vision.size; + uint16_t m; + m = c; + + msg.size = m; + } + + { + uint32_t c = n.navdata_vision.vision_state; + uint32_t m; + m = c; + + msg.vision_state = m; + } + + { + int32_t c = n.navdata_vision.vision_misc; + int32_t m; + m = c; + + msg.vision_misc = m; + } + + { + float32_t c = n.navdata_vision.vision_phi_trim; + float32_t m; + m = c; + + msg.vision_phi_trim = m; + } + + { + float32_t c = n.navdata_vision.vision_phi_ref_prop; + float32_t m; + m = c; + + msg.vision_phi_ref_prop = m; + } + + { + float32_t c = n.navdata_vision.vision_theta_trim; + float32_t m; + m = c; + + msg.vision_theta_trim = m; + } + + { + float32_t c = n.navdata_vision.vision_theta_ref_prop; + float32_t m; + m = c; + + msg.vision_theta_ref_prop = m; + } + + { + int32_t c = n.navdata_vision.new_raw_picture; + int32_t m; + m = c; + + msg.new_raw_picture = m; + } + + { + float32_t c = n.navdata_vision.theta_capture; + float32_t m; + m = c; + + msg.theta_capture = m; + } + + { + float32_t c = n.navdata_vision.phi_capture; + float32_t m; + m = c; + + msg.phi_capture = m; + } + + { + float32_t c = n.navdata_vision.psi_capture; + float32_t m; + m = c; + + msg.psi_capture = m; + } + + { + int32_t c = n.navdata_vision.altitude_capture; + int32_t m; + m = c; + + msg.altitude_capture = m; + } + + { + uint32_t c = n.navdata_vision.time_capture; + uint32_t m; + m = c; + + msg.time_capture = m; + } + + { + velocities_t c = n.navdata_vision.body_v; + ardrone_autonomy::vector31 m; + m.x = c.x; + m.y = c.y; + m.z = c.z; + + msg.body_v = m; + } + + { + float32_t c = n.navdata_vision.delta_phi; + float32_t m; + m = c; + + msg.delta_phi = m; + } + + { + float32_t c = n.navdata_vision.delta_theta; + float32_t m; + m = c; + + msg.delta_theta = m; + } + + { + float32_t c = n.navdata_vision.delta_psi; + float32_t m; + m = c; + + msg.delta_psi = m; + } + + { + uint32_t c = n.navdata_vision.gold_defined; + uint32_t m; + m = c; + + msg.gold_defined = m; + } + + { + uint32_t c = n.navdata_vision.gold_reset; + uint32_t m; + m = c; + + msg.gold_reset = m; + } + + { + float32_t c = n.navdata_vision.gold_x; + float32_t m; + m = c; + + msg.gold_x = m; + } + + { + float32_t c = n.navdata_vision.gold_y; + float32_t m; + m = c; + + msg.gold_y = m; + } + + pub_navdata_vision.publish(msg); + } + + if(enabled_navdata_vision_perf && pub_navdata_vision_perf.getNumSubscribers()>0) + { + ardrone_autonomy::navdata_vision_perf msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + + { + uint16_t c = n.navdata_vision_perf.tag; + uint16_t m; + m = c; + + msg.tag = m; + } + + { + uint16_t c = n.navdata_vision_perf.size; + uint16_t m; + m = c; + + msg.size = m; + } + + { + float32_t c = n.navdata_vision_perf.time_corners; + float32_t m; + m = c; + + msg.time_corners = m; + } + + { + float32_t c = n.navdata_vision_perf.time_compute; + float32_t m; + m = c; + + msg.time_compute = m; + } + + { + float32_t c = n.navdata_vision_perf.time_tracking; + float32_t m; + m = c; + + msg.time_tracking = m; + } + + { + float32_t c = n.navdata_vision_perf.time_trans; + float32_t m; + m = c; + + msg.time_trans = m; + } + + { + float32_t c = n.navdata_vision_perf.time_update; + float32_t m; + m = c; + + msg.time_update = m; + } + + for(int i=0; i0) + { + ardrone_autonomy::navdata_trackers_send msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + + { + uint16_t c = n.navdata_trackers_send.tag; + uint16_t m; + m = c; + + msg.tag = m; + } + + { + uint16_t c = n.navdata_trackers_send.size; + uint16_t m; + m = c; + + msg.size = m; + } + + for(int i=0; i0) + { + ardrone_autonomy::navdata_vision_detect msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + + { + uint16_t c = n.navdata_vision_detect.tag; + uint16_t m; + m = c; + + msg.tag = m; + } + + { + uint16_t c = n.navdata_vision_detect.size; + uint16_t m; + m = c; + + msg.size = m; + } + + { + uint32_t c = n.navdata_vision_detect.nb_detected; + uint32_t m; + m = c; + + msg.nb_detected = m; + } + + for(int i=0; i0) + { + ardrone_autonomy::navdata_watchdog msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + + { + uint16_t c = n.navdata_watchdog.tag; + uint16_t m; + m = c; + + msg.tag = m; + } + + { + uint16_t c = n.navdata_watchdog.size; + uint16_t m; + m = c; + + msg.size = m; + } + + pub_navdata_watchdog.publish(msg); + } + + if(enabled_navdata_adc_data_frame && pub_navdata_adc_data_frame.getNumSubscribers()>0) + { + ardrone_autonomy::navdata_adc_data_frame msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + + { + uint16_t c = n.navdata_adc_data_frame.tag; + uint16_t m; + m = c; + + msg.tag = m; + } + + { + uint16_t c = n.navdata_adc_data_frame.size; + uint16_t m; + m = c; + + msg.size = m; + } + + { + uint32_t c = n.navdata_adc_data_frame.version; + uint32_t m; + m = c; + + msg.version = m; + } + + for(int i=0; i<32; i++) + { + uint8_t c = n.navdata_adc_data_frame.data_frame[i]; + uint8_t m; + m = c; + + msg.data_frame.push_back(m); + } + + pub_navdata_adc_data_frame.publish(msg); + } + + if(enabled_navdata_video_stream && pub_navdata_video_stream.getNumSubscribers()>0) + { + ardrone_autonomy::navdata_video_stream msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + + { + uint16_t c = n.navdata_video_stream.tag; + uint16_t m; + m = c; + + msg.tag = m; + } + + { + uint16_t c = n.navdata_video_stream.size; + uint16_t m; + m = c; + + msg.size = m; + } + + { + uint8_t c = n.navdata_video_stream.quant; + uint8_t m; + m = c; + + msg.quant = m; + } + + { + uint32_t c = n.navdata_video_stream.frame_size; + uint32_t m; + m = c; + + msg.frame_size = m; + } + + { + uint32_t c = n.navdata_video_stream.frame_number; + uint32_t m; + m = c; + + msg.frame_number = m; + } + + { + uint32_t c = n.navdata_video_stream.atcmd_ref_seq; + uint32_t m; + m = c; + + msg.atcmd_ref_seq = m; + } + + { + uint32_t c = n.navdata_video_stream.atcmd_mean_ref_gap; + uint32_t m; + m = c; + + msg.atcmd_mean_ref_gap = m; + } + + { + float32_t c = n.navdata_video_stream.atcmd_var_ref_gap; + float32_t m; + m = c; + + msg.atcmd_var_ref_gap = m; + } + + { + uint32_t c = n.navdata_video_stream.atcmd_ref_quality; + uint32_t m; + m = c; + + msg.atcmd_ref_quality = m; + } + + { + uint32_t c = n.navdata_video_stream.desired_bitrate; + uint32_t m; + m = c; + + msg.desired_bitrate = m; + } + + { + int32_t c = n.navdata_video_stream.data2; + int32_t m; + m = c; + + msg.data2 = m; + } + + { + int32_t c = n.navdata_video_stream.data3; + int32_t m; + m = c; + + msg.data3 = m; + } + + { + int32_t c = n.navdata_video_stream.data4; + int32_t m; + m = c; + + msg.data4 = m; + } + + { + int32_t c = n.navdata_video_stream.data5; + int32_t m; + m = c; + + msg.data5 = m; + } + + { + uint32_t c = n.navdata_video_stream.fifo_queue_level; + uint32_t m; + m = c; + + msg.fifo_queue_level = m; + } + + pub_navdata_video_stream.publish(msg); + } + + if(enabled_navdata_games && pub_navdata_games.getNumSubscribers()>0) + { + ardrone_autonomy::navdata_games msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + + { + uint16_t c = n.navdata_games.tag; + uint16_t m; + m = c; + + msg.tag = m; + } + + { + uint16_t c = n.navdata_games.size; + uint16_t m; + m = c; + + msg.size = m; + } + + { + uint32_t c = n.navdata_games.double_tap_counter; + uint32_t m; + m = c; + + msg.double_tap_counter = m; + } + + { + uint32_t c = n.navdata_games.finish_line_counter; + uint32_t m; + m = c; + + msg.finish_line_counter = m; + } + + pub_navdata_games.publish(msg); + } + + if(enabled_navdata_pressure_raw && pub_navdata_pressure_raw.getNumSubscribers()>0) + { + ardrone_autonomy::navdata_pressure_raw msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + + { + uint16_t c = n.navdata_pressure_raw.tag; + uint16_t m; + m = c; + + msg.tag = m; + } + + { + uint16_t c = n.navdata_pressure_raw.size; + uint16_t m; + m = c; + + msg.size = m; + } + + { + int32_t c = n.navdata_pressure_raw.up; + int32_t m; + m = c; + + msg.up = m; + } + + { + int16_t c = n.navdata_pressure_raw.ut; + int16_t m; + m = c; + + msg.ut = m; + } + + { + int32_t c = n.navdata_pressure_raw.Temperature_meas; + int32_t m; + m = c; + + msg.Temperature_meas = m; + } + + { + int32_t c = n.navdata_pressure_raw.Pression_meas; + int32_t m; + m = c; + + msg.Pression_meas = m; + } + + pub_navdata_pressure_raw.publish(msg); + } + + if(enabled_navdata_magneto && pub_navdata_magneto.getNumSubscribers()>0) + { + ardrone_autonomy::navdata_magneto msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + + { + uint16_t c = n.navdata_magneto.tag; + uint16_t m; + m = c; + + msg.tag = m; + } + + { + uint16_t c = n.navdata_magneto.size; + uint16_t m; + m = c; + + msg.size = m; + } + + { + int16_t c = n.navdata_magneto.mx; + int16_t m; + m = c; + + msg.mx = m; + } + + { + int16_t c = n.navdata_magneto.my; + int16_t m; + m = c; + + msg.my = m; + } + + { + int16_t c = n.navdata_magneto.mz; + int16_t m; + m = c; + + msg.mz = m; + } + + { + vector31_t c = n.navdata_magneto.magneto_raw; + ardrone_autonomy::vector31 m; + m.x = c.x; + m.y = c.y; + m.z = c.z; + + msg.magneto_raw = m; + } + + { + vector31_t c = n.navdata_magneto.magneto_rectified; + ardrone_autonomy::vector31 m; + m.x = c.x; + m.y = c.y; + m.z = c.z; + + msg.magneto_rectified = m; + } + + { + vector31_t c = n.navdata_magneto.magneto_offset; + ardrone_autonomy::vector31 m; + m.x = c.x; + m.y = c.y; + m.z = c.z; + + msg.magneto_offset = m; + } + + { + float32_t c = n.navdata_magneto.heading_unwrapped; + float32_t m; + m = c; + + msg.heading_unwrapped = m; + } + + { + float32_t c = n.navdata_magneto.heading_gyro_unwrapped; + float32_t m; + m = c; + + msg.heading_gyro_unwrapped = m; + } + + { + float32_t c = n.navdata_magneto.heading_fusion_unwrapped; + float32_t m; + m = c; + + msg.heading_fusion_unwrapped = m; + } + + { + char c = n.navdata_magneto.magneto_calibration_ok; + char m; + m = c; + + msg.magneto_calibration_ok = m; + } + + { + uint32_t c = n.navdata_magneto.magneto_state; + uint32_t m; + m = c; + + msg.magneto_state = m; + } + + { + float32_t c = n.navdata_magneto.magneto_radius; + float32_t m; + m = c; + + msg.magneto_radius = m; + } + + { + float32_t c = n.navdata_magneto.error_mean; + float32_t m; + m = c; + + msg.error_mean = m; + } + + { + float32_t c = n.navdata_magneto.error_var; + float32_t m; + m = c; + + msg.error_var = m; + } + + pub_navdata_magneto.publish(msg); + } + + if(enabled_navdata_wind_speed && pub_navdata_wind_speed.getNumSubscribers()>0) + { + ardrone_autonomy::navdata_wind_speed msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + + { + uint16_t c = n.navdata_wind_speed.tag; + uint16_t m; + m = c; + + msg.tag = m; + } + + { + uint16_t c = n.navdata_wind_speed.size; + uint16_t m; + m = c; + + msg.size = m; + } + + { + float32_t c = n.navdata_wind_speed.wind_speed; + float32_t m; + m = c; + + msg.wind_speed = m; + } + + { + float32_t c = n.navdata_wind_speed.wind_angle; + float32_t m; + m = c; + + msg.wind_angle = m; + } + + { + float32_t c = n.navdata_wind_speed.wind_compensation_theta; + float32_t m; + m = c; + + msg.wind_compensation_theta = m; + } + + { + float32_t c = n.navdata_wind_speed.wind_compensation_phi; + float32_t m; + m = c; + + msg.wind_compensation_phi = m; + } + + { + float32_t c = n.navdata_wind_speed.state_x1; + float32_t m; + m = c; + + msg.state_x1 = m; + } + + { + float32_t c = n.navdata_wind_speed.state_x2; + float32_t m; + m = c; + + msg.state_x2 = m; + } + + { + float32_t c = n.navdata_wind_speed.state_x3; + float32_t m; + m = c; + + msg.state_x3 = m; + } + + { + float32_t c = n.navdata_wind_speed.state_x4; + float32_t m; + m = c; + + msg.state_x4 = m; + } + + { + float32_t c = n.navdata_wind_speed.state_x5; + float32_t m; + m = c; + + msg.state_x5 = m; + } + + { + float32_t c = n.navdata_wind_speed.state_x6; + float32_t m; + m = c; + + msg.state_x6 = m; + } + + { + float32_t c = n.navdata_wind_speed.magneto_debug1; + float32_t m; + m = c; + + msg.magneto_debug1 = m; + } + + { + float32_t c = n.navdata_wind_speed.magneto_debug2; + float32_t m; + m = c; + + msg.magneto_debug2 = m; + } + + { + float32_t c = n.navdata_wind_speed.magneto_debug3; + float32_t m; + m = c; + + msg.magneto_debug3 = m; + } + + pub_navdata_wind_speed.publish(msg); + } + + if(enabled_navdata_kalman_pressure && pub_navdata_kalman_pressure.getNumSubscribers()>0) + { + ardrone_autonomy::navdata_kalman_pressure msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + + { + uint16_t c = n.navdata_kalman_pressure.tag; + uint16_t m; + m = c; + + msg.tag = m; + } + + { + uint16_t c = n.navdata_kalman_pressure.size; + uint16_t m; + m = c; + + msg.size = m; + } + + { + float32_t c = n.navdata_kalman_pressure.offset_pressure; + float32_t m; + m = c; + + msg.offset_pressure = m; + } + + { + float32_t c = n.navdata_kalman_pressure.est_z; + float32_t m; + m = c; + + msg.est_z = m; + } + + { + float32_t c = n.navdata_kalman_pressure.est_zdot; + float32_t m; + m = c; + + msg.est_zdot = m; + } + + { + float32_t c = n.navdata_kalman_pressure.est_bias_PWM; + float32_t m; + m = c; + + msg.est_bias_PWM = m; + } + + { + float32_t c = n.navdata_kalman_pressure.est_biais_pression; + float32_t m; + m = c; + + msg.est_biais_pression = m; + } + + { + float32_t c = n.navdata_kalman_pressure.offset_US; + float32_t m; + m = c; + + msg.offset_US = m; + } + + { + float32_t c = n.navdata_kalman_pressure.prediction_US; + float32_t m; + m = c; + + msg.prediction_US = m; + } + + { + float32_t c = n.navdata_kalman_pressure.cov_alt; + float32_t m; + m = c; + + msg.cov_alt = m; + } + + { + float32_t c = n.navdata_kalman_pressure.cov_PWM; + float32_t m; + m = c; + + msg.cov_PWM = m; + } + + { + float32_t c = n.navdata_kalman_pressure.cov_vitesse; + float32_t m; + m = c; + + msg.cov_vitesse = m; + } + + { + bool_t c = n.navdata_kalman_pressure.bool_effet_sol; + bool_t m; + m = c; + + msg.bool_effet_sol = m; + } + + { + float32_t c = n.navdata_kalman_pressure.somme_inno; + float32_t m; + m = c; + + msg.somme_inno = m; + } + + { + bool_t c = n.navdata_kalman_pressure.flag_rejet_US; + bool_t m; + m = c; + + msg.flag_rejet_US = m; + } + + { + float32_t c = n.navdata_kalman_pressure.u_multisinus; + float32_t m; + m = c; + + msg.u_multisinus = m; + } + + { + float32_t c = n.navdata_kalman_pressure.gaz_altitude; + float32_t m; + m = c; + + msg.gaz_altitude = m; + } + + { + bool_t c = n.navdata_kalman_pressure.Flag_multisinus; + bool_t m; + m = c; + + msg.Flag_multisinus = m; + } + + { + bool_t c = n.navdata_kalman_pressure.Flag_multisinus_debut; + bool_t m; + m = c; + + msg.Flag_multisinus_debut = m; + } + + pub_navdata_kalman_pressure.publish(msg); + } + + if(enabled_navdata_hdvideo_stream && pub_navdata_hdvideo_stream.getNumSubscribers()>0) + { + ardrone_autonomy::navdata_hdvideo_stream msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + + { + uint16_t c = n.navdata_hdvideo_stream.tag; + uint16_t m; + m = c; + + msg.tag = m; + } + + { + uint16_t c = n.navdata_hdvideo_stream.size; + uint16_t m; + m = c; + + msg.size = m; + } + + { + uint32_t c = n.navdata_hdvideo_stream.hdvideo_state; + uint32_t m; + m = c; + + msg.hdvideo_state = m; + } + + { + uint32_t c = n.navdata_hdvideo_stream.storage_fifo_nb_packets; + uint32_t m; + m = c; + + msg.storage_fifo_nb_packets = m; + } + + { + uint32_t c = n.navdata_hdvideo_stream.storage_fifo_size; + uint32_t m; + m = c; + + msg.storage_fifo_size = m; + } + + { + uint32_t c = n.navdata_hdvideo_stream.usbkey_size; + uint32_t m; + m = c; + + msg.usbkey_size = m; + } + + { + uint32_t c = n.navdata_hdvideo_stream.usbkey_freespace; + uint32_t m; + m = c; + + msg.usbkey_freespace = m; + } + + { + uint32_t c = n.navdata_hdvideo_stream.frame_number; + uint32_t m; + m = c; + + msg.frame_number = m; + } + + { + uint32_t c = n.navdata_hdvideo_stream.usbkey_remaining_time; + uint32_t m; + m = c; + + msg.usbkey_remaining_time = m; + } + + pub_navdata_hdvideo_stream.publish(msg); + } + + if(enabled_navdata_wifi && pub_navdata_wifi.getNumSubscribers()>0) + { + ardrone_autonomy::navdata_wifi msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + + { + uint16_t c = n.navdata_wifi.tag; + uint16_t m; + m = c; + + msg.tag = m; + } + + { + uint16_t c = n.navdata_wifi.size; + uint16_t m; + m = c; + + msg.size = m; + } + + { + uint32_t c = n.navdata_wifi.link_quality; + uint32_t m; + m = c; + + msg.link_quality = m; + } + + pub_navdata_wifi.publish(msg); + } + + if(enabled_navdata_zimmu_3000 && pub_navdata_zimmu_3000.getNumSubscribers()>0) + { + ardrone_autonomy::navdata_zimmu_3000 msg; + msg.drone_time = ((double)ardrone_time_to_usec(n.navdata_time.time))/1000000.0; + + { + uint16_t c = n.navdata_zimmu_3000.tag; + uint16_t m; + m = c; + + msg.tag = m; + } + + { + uint16_t c = n.navdata_zimmu_3000.size; + uint16_t m; + m = c; + + msg.size = m; + } + + { + int32_t c = n.navdata_zimmu_3000.vzimmuLSB; + int32_t m; + m = c; + + msg.vzimmuLSB = m; + } + + { + float32_t c = n.navdata_zimmu_3000.vzfind; + float32_t m; + m = c; + + msg.vzfind = m; + } + + pub_navdata_zimmu_3000.publish(msg); + } + + +} +#endif -NavdataStructStart(navdata_hdvideo_stream_t,navdata_hdvideo_stream) -NavdataStructMember(navdata_hdvideo_stream_t,navdata_hdvideo_stream,uint16_t,uint16,tag) -NavdataStructMember(navdata_hdvideo_stream_t,navdata_hdvideo_stream,uint16_t,uint16,size) -NavdataStructMember(navdata_hdvideo_stream_t,navdata_hdvideo_stream,uint32_t,uint32,hdvideo_state) -NavdataStructMember(navdata_hdvideo_stream_t,navdata_hdvideo_stream,uint32_t,uint32,storage_fifo_nb_packets) -NavdataStructMember(navdata_hdvideo_stream_t,navdata_hdvideo_stream,uint32_t,uint32,storage_fifo_size) -NavdataStructMember(navdata_hdvideo_stream_t,navdata_hdvideo_stream,uint32_t,uint32,usbkey_size) -NavdataStructMember(navdata_hdvideo_stream_t,navdata_hdvideo_stream,uint32_t,uint32,usbkey_freespace) -NavdataStructMember(navdata_hdvideo_stream_t,navdata_hdvideo_stream,uint32_t,uint32,frame_number) -NavdataStructMember(navdata_hdvideo_stream_t,navdata_hdvideo_stream,uint32_t,uint32,usbkey_remaining_time) -NavdataStructEnd(navdata_hdvideo_stream_t,navdata_hdvideo_stream) -NavdataStructStart(navdata_wifi_t,navdata_wifi) -NavdataStructMember(navdata_wifi_t,navdata_wifi,uint16_t,uint16,tag) -NavdataStructMember(navdata_wifi_t,navdata_wifi,uint16_t,uint16,size) -NavdataStructMember(navdata_wifi_t,navdata_wifi,uint32_t,uint32,link_quality) -NavdataStructEnd(navdata_wifi_t,navdata_wifi) -NavdataStructStart(navdata_zimmu_3000_t,navdata_zimmu_3000) -NavdataStructMember(navdata_zimmu_3000_t,navdata_zimmu_3000,uint16_t,uint16,tag) -NavdataStructMember(navdata_zimmu_3000_t,navdata_zimmu_3000,uint16_t,uint16,size) -NavdataStructMember(navdata_zimmu_3000_t,navdata_zimmu_3000,int32_t,int32,vzimmuLSB) -NavdataStructMember(navdata_zimmu_3000_t,navdata_zimmu_3000,float32_t,float32,vzfind) -NavdataStructEnd(navdata_zimmu_3000_t,navdata_zimmu_3000) diff --git a/src/ardrone_driver.cpp b/src/ardrone_driver.cpp index f67163d..545e1d3 100644 --- a/src/ardrone_driver.cpp +++ b/src/ardrone_driver.cpp @@ -562,6 +562,7 @@ void ARDroneDriver::publish_navdata() { // Thread safe copy of interesting Navdata data vp_os_mutex_lock(&navdata_lock); + navdata_raw = shared_raw_navdata; navdata_detect = shared_navdata_detect; navdata_phys = shared_navdata_phys; navdata = shared_navdata; @@ -614,6 +615,9 @@ void ARDroneDriver::publish_navdata() navdata.vz -= vel_bias[2]; } + + PublishNavdataTypes(navdata_raw); // This is defined in the template NavdataMessageDefinitions.h template file + if ((navdata_pub.getNumSubscribers() == 0) && (imu_pub.getNumSubscribers() == 0) && (mag_pub.getNumSubscribers() == 0)) return; // why bother, no one is listening. const ros::Time _now = ros::Time::now(); @@ -729,6 +733,10 @@ void ARDroneDriver::publish_navdata() imu_pub.publish(imu_msg); } +#define NAVDATA_STRUCTS_SOURCE +#include "NavdataMessageDefinitions.h" +#undef NAVDATA_STRUCTS_SOURCE + void ARDroneDriver::publish_tf() { tf_base_front.stamp_ = ros::Time::now(); diff --git a/src/ardrone_driver.h b/src/ardrone_driver.h index 22793f5..50e5a99 100644 --- a/src/ardrone_driver.h +++ b/src/ardrone_driver.h @@ -16,12 +16,18 @@ #include #include +#define NAVDATA_STRUCTS_INCLUDES +#include "NavdataMessageDefinitions.h" +#undef NAVDATA_STRUCTS_INCLUDES + #define _DEG2RAD 0.01745331111 #define _RAD2DEG 57.2957184819 #define DRIVER_USERNAME "ardrone_driver" #define DRIVER_APPNAME "ardrone_driver" +#define CAMERA_QUEUE_SIZE (10) +#define NAVDATA_QUEUE_SIZE (25) enum ROOT_FRAME { @@ -104,6 +110,11 @@ private: navdata_magneto_t navdata_magneto; navdata_wind_speed_t navdata_wind; navdata_time_t arnavtime; + navdata_unpacked_t navdata_raw; + + #define NAVDATA_STRUCTS_HEADER + #include "NavdataMessageDefinitions.h" + #undef NAVDATA_STRUCTS_HEADER /* * TF Frames