00001 /* 00002 00003 Copyright (c) 2011, Ascending Technologies GmbH 00004 All rights reserved. 00005 00006 Redistribution and use in source and binary forms, with or without 00007 modification, are permitted provided that the following conditions are met: 00008 00009 * Redistributions of source code must retain the above copyright notice, 00010 this list of conditions and the following disclaimer. 00011 * Redistributions in binary form must reproduce the above copyright 00012 notice, this list of conditions and the following disclaimer in the 00013 documentation and/or other materials provided with the distribution. 00014 00015 THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND ANY 00016 EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00017 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00018 DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE FOR ANY 00019 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00020 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 00021 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00022 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00023 LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 00024 OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 00025 DAMAGE. 00026 00027 */ 00028 00029 #ifndef LL_HL_COMM_ 00030 #define LL_HL_COMM_ 00031 00032 //system flags 00033 #define SF_PAGE_BIT1 0x01 00034 #define SF_PAGE_BIT2 0x02 00035 #define SF_SSP_ACK 0x04 00036 #define SF_GPS_NEW 0x08 00037 #define SF_HL_CONTROL_ENABLED 0x10 00038 #define SF_DIRECT_MOTOR_CONTROL 0x20 00039 #define SF_WAYPOINT_MODE 0x40 00040 #define SF_DIRECT_MOTOR_CONTROL_INDIVIDUAL 0x80 00041 #define SF_SDK_DISABLE_MOTORONOFF_BY_STICK 0x100 00042 #define SF_NEW_SDK 0x8000 00043 00044 00045 //ctrl_flags 00046 //scientific control 00047 #define HL_CTRL_PITCH 0x01 00048 #define HL_CTRL_ROLL 0x02 00049 #define HL_CTRL_YAW 0x04 00050 #define HL_CTRL_THRUST 0x08 00051 #define HL_CTRL_HEIGHT_ENABLED 0x10 00052 #define HL_CTRL_GPS_ENABLED 0x20 00053 00054 //direct motor control 00055 #define HL_CTRL_MOTORS_ONOFF_BY_RC 0x01 00056 00057 00058 #define WP_CMD_SINGLE_WP_PART1 0x81 //internal use! 00059 #define WP_CMD_SINGLE_WP_PART2 0x82 //internal use! 00060 00061 //Slow Data Channel defines 00062 //declination data [short] 00063 #define SDC_DECLINATION 0x01 00064 //declination data [short] 00065 #define SDC_INCLINATION 0x12 00066 00067 //Slow Data Up Channel defines 00068 #define SUDC_NONE 0x00 00069 //flight time. flighttime in slowDataUpChannelShort 00070 #define SUDC_FLIGHTTIME 0x01 00071 //set camera type. Cameratype in slowDataUpChannelShort 00072 #define SUDC_SETCAMERA 0x02 00073 //set payload options. payloadoptions in slowDataUpChannelShort 00074 #define SUDC_SETPAYLOADOPTIONS 0x03 00075 //set camera mount roll angle calibration 00076 #define SUDC_SETCAMERAROLLANGLECALIB 0x04 00077 //set camera mount roll angle calibration and store to eeprom 00078 #define SUDC_SETCAMERAROLLANGLECALIBANDSTORE 0x05 00079 //navigation status 00080 #define SUDC_NAVSTATUS 0x06 00081 //distance to waypoint 00082 #define SUDC_DISTTOWP 0x07 00083 //waypoint ack trigger 00084 #define SUDC_WPACKTRIGGER 0x08 00085 00086 void LL_write_ctrl_data(char); 00087 int HL2LL_write_cycle(void); 00088 inline void SSP_rx_handler_HL(unsigned char); 00089 inline void SSP_data_distribution_HL(void); 00090 struct LL_ATTITUDE_DATA 00091 { 00092 unsigned short system_flags; //GPS data acknowledge, etc. 00093 00094 short angle_pitch; //angles [deg*100] 00095 short angle_roll; 00096 unsigned short angle_yaw; 00097 00098 short angvel_pitch; //angular velocities; bias-free [0.015°/s] 00099 short angvel_roll; 00100 short angvel_yaw; 00101 00102 //<-- 14 bytes @ 1kHz 00103 //--> 3x 26 bytes @ 333 Hz 00104 //=> total = 40 bytes @ 1 kHz 00105 //-----------------------------PAGE0 00106 unsigned char RC_data[10]; //8 channels @ 10 bit 00107 00108 int latitude_best_estimate; //GPS data fused with all other sensors 00109 int longitude_best_estimate; 00110 short acc_x; //accelerations [mg] 00111 short acc_y; 00112 short acc_z; 00113 00114 unsigned short temp_gyro; 00115 //-----------------------------PAGE1 00116 unsigned char motor_data[16]; //speed 0..7, PWM 0..7 00117 00118 short speed_x_best_estimate; 00119 short speed_y_best_estimate; 00120 int height; //height [mm] 00121 short dheight; //differentiated height[mm/s] 00122 //------------------------------PAGE2 00123 short mag_x; 00124 short mag_y; 00125 short mag_z; 00126 00127 short cam_angle_pitch; 00128 short cam_angle_roll; 00129 short cam_status; 00130 00131 short battery_voltage1; 00132 short battery_voltage2; 00133 short flightMode; 00134 short slowDataUpChannelDataShort; //former flight_time 00135 short cpu_load; 00136 short status; 00137 short status2; //Bits 7..1: slowDataUpChannelSelect (7bit) Bit0:flying Bit15..8:active Motors 00138 00139 }; 00140 00141 extern struct LL_ATTITUDE_DATA LL_1khz_attitude_data; 00142 00143 struct LL_CONTROL_INPUT 00144 { 00145 unsigned short system_flags; 00146 //bit 0: page_select 00147 //bit 1: reserved (page_select) 00148 //bit 2: SSP_ack 00149 //bit 3: GPS new 00150 //bit 4: HL controller enabled 00151 //bit 5: 0 -> "scientific" commands 00152 // 1 -> direct motor commands 00153 //bit 6: waypoint mode 00154 00155 unsigned short ctrl_flags; 00156 //bit 0..3: 00157 // pitch, roll, yaw, thrust enable bits 00158 //bit 4: height control enabled 00159 //bit 5: GPS_control enabled 00160 //bit 8..15: waypoint command if waypoint mode is active or POI options if POI mode is active 00161 00162 short pitch, roll, yaw, thrust; //"scientific interface" 00163 unsigned char direct_motor_control[8]; //direct motor commands: pitch, roll, yaw, throttle, 4xDNC 00164 //or motor 0..7 (Falcon) 00165 00166 //<-- 20 bytes @ 1kHz 00167 //--> 2x18 bytes @ 500 Hz 00168 //=> total = 38 bytes @ 1kHz 00169 00170 int latitude; //data received from GPS-unit 00171 int longitude; 00172 int height; 00173 short speed_x; 00174 short speed_y; 00175 short status; 00176 //----------------------------- 00177 00178 unsigned short hor_accuracy; 00179 unsigned short vert_accuracy; 00180 unsigned short speed_accuracy; 00181 unsigned short numSV; 00182 unsigned short heading; 00183 short battery_voltage_1, battery_voltage_2; //battery voltage read by HL-ADC [mV] 00184 unsigned char slowDataChannelSelect; // these three vars define a slow data transfer channel. the select byte defines which data is in the data channel 00185 unsigned char slowDataChannelDataChar; 00186 short slowDataChannelDataShort; 00187 }; 00188 extern struct LL_CONTROL_INPUT LL_1khz_control_input; 00189 00190 00191 00192 #endif /*LL_HL_COMM_*/ 00193 00194 00195 00196 /* old file 00197 #ifndef LL_HL_COMM_ 00198 #define LL_HL_COMM_ 00199 00200 //system flags 00201 #define SF_PAGE_BIT1 0x01 00202 #define SF_PAGE_BIT2 0x02 00203 #define SF_SSP_ACK 0x04 00204 #define SF_GPS_NEW 0x08 00205 #define SF_HL_CONTROL_ENABLED 0x10 00206 #define SF_DIRECT_MOTOR_CONTROL 0x20 00207 #define SF_WAYPOINT_MODE 0x40 00208 00209 //ctrl_flags 00210 //scientific control 00211 #define HL_CTRL_PITCH 0x01 00212 #define HL_CTRL_ROLL 0x02 00213 #define HL_CTRL_YAW 0x04 00214 #define HL_CTRL_THRUST 0x08 00215 #define HL_CTRL_HEIGHT_ENABLED 0x10 00216 #define HL_CTRL_GPS_ENABLED 0x20 00217 //direct motor control 00218 #define HL_CTRL_MOTORS_ONOFF_BY_RC 0x01 00219 00220 00221 void LL_write_ctrl_data(char); 00222 int HL2LL_write_cycle(void); 00223 inline void SSP_rx_handler_HL(unsigned char); 00224 inline void SSP_data_distribution_HL(void); 00225 00226 struct LL_ATTITUDE_DATA 00227 { 00228 unsigned short system_flags; //GPS data acknowledge, etc. 00229 00230 short angle_pitch; //angles [deg*100] 00231 short angle_roll; 00232 unsigned short angle_yaw; 00233 00234 short angvel_pitch; //angular velocities; bias-free [0.015°/s] 00235 short angvel_roll; 00236 short angvel_yaw; 00237 00238 //<-- 14 bytes @ 1kHz 00239 //--> 3x 26 bytes @ 333 Hz 00240 //=> total = 40 bytes @ 1 kHz 00241 //-----------------------------PAGE0 00242 unsigned char RC_data[10]; //8 channels @ 10 bit 00243 00244 int latitude_best_estimate; //GPS data fused with all other sensors 00245 int longitude_best_estimate; 00246 short acc_x; //accelerations [mg] 00247 short acc_y; 00248 short acc_z; 00249 00250 short dummy_333Hz_1; 00251 //-----------------------------PAGE1 00252 unsigned char motor_data[16]; //speed 0..7, PWM 0..7 00253 00254 short speed_x_best_estimate; 00255 short speed_y_best_estimate; 00256 int height; //height [mm] 00257 short dheight; //differentiated height[mm/s] 00258 //------------------------------PAGE2 00259 short mag_x; 00260 short mag_y; 00261 short mag_z; 00262 00263 short cam_angle_pitch; 00264 short cam_angle_roll; 00265 short cam_status; 00266 00267 short battery_voltage1; 00268 short battery_voltage2; 00269 short flightMode; 00270 short flight_time; 00271 short cpu_load; 00272 short status; 00273 short status2; 00274 00275 }; 00276 00277 extern struct LL_ATTITUDE_DATA LL_1khz_attitude_data; 00278 00279 struct LL_CONTROL_INPUT 00280 { 00281 unsigned short system_flags; 00282 //bit 0: page_select 00283 //bit 1: reserved (page_select) 00284 //bit 2: SSP_ack 00285 //bit 3: GPS new 00286 //bit 4: HL controller enabled 00287 //bit 5: 0 -> "scientific" commands 00288 // 1 -> direct motor commands 00289 //bit 6: waypoint mode 00290 00291 unsigned short ctrl_flags; 00292 //bit 0..3: 00293 // pitch, roll, yaw, height enable bits 00294 //bit 4: height control enabled 00295 //bit 5: yaw_control enabled 00296 00297 short pitch, roll, yaw, thrust; //"scientific interface" 00298 unsigned char direct_motor_control[8]; //direct motor commands: pitch, roll, yaw, throttle, 4xDNC 00299 //or motor 0..7 (Falcon) 00300 00301 //<-- 20 bytes @ 1kHz 00302 //--> 2x18 bytes @ 500 Hz 00303 //=> total = 38 bytes @ 1kHz 00304 00305 int latitude; //data received from GPS-unit 00306 int longitude; 00307 int height; 00308 short speed_x; 00309 short speed_y; 00310 short status; 00311 //----------------------------- 00312 00313 unsigned short hor_accuracy; 00314 unsigned short vert_accuracy; 00315 unsigned short speed_accuracy; 00316 unsigned short numSV; 00317 unsigned short heading; 00318 short battery_voltage_1, battery_voltage_2; //battery voltage read by HL-ADC [mV] 00319 short dummy_500Hz_2; 00320 short dummy_500Hz_3; 00321 }; 00322 00323 extern struct LL_CONTROL_INPUT LL_1khz_control_input; 00324 00325 00326 #endif //LL_HL_COMM_ 00327 end old file */