00001 /* 00002 00003 AscTec AutoPilot HL SDK v2.0 00004 00005 Copyright (c) 2011, Ascending Technologies GmbH 00006 All rights reserved. 00007 00008 Redistribution and use in source and binary forms, with or without 00009 modification, are permitted provided that the following conditions are met: 00010 00011 * Redistributions of source code must retain the above copyright notice, 00012 this list of conditions and the following disclaimer. 00013 * Redistributions in binary form must reproduce the above copyright 00014 notice, this list of conditions and the following disclaimer in the 00015 documentation and/or other materials provided with the distribution. 00016 00017 THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND ANY 00018 EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00019 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00020 DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE FOR ANY 00021 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00022 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 00023 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00024 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00025 LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 00026 OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH 00027 DAMAGE. 00028 00029 */ 00030 00031 #include "main.h" 00032 #include "sdk.h" 00033 #include "LL_HL_comm.h" 00034 #include "gpsmath.h" 00035 00036 struct WO_SDK_STRUCT WO_SDK; 00037 struct WO_CTRL_INPUT WO_CTRL_Input; 00038 struct RO_RC_DATA RO_RC_Data; 00039 struct RO_ALL_DATA RO_ALL_Data; 00040 struct WO_DIRECT_MOTOR_CONTROL WO_Direct_Motor_Control; 00041 struct WO_DIRECT_INDIVIDUAL_MOTOR_CONTROL WO_Direct_Individual_Motor_Control; 00042 00043 void SDK_EXAMPLE_direct_individual_motor_commands(void); 00044 void SDK_EXAMPLE_direct_motor_commands_with_standard_output_mapping(void); 00045 void SDK_EXAMPLE_attitude_commands(void); 00046 void SDK_EXAMPLE_gps_waypoint_control(void); 00047 int SDK_EXAMPLE_turn_motors_on(void); 00048 int SDK_EXAMPLE_turn_motors_off(void); 00049 00050 /* SDK_mainloop(void) is triggered @ 1kHz. 00051 * 00052 * RO_(Read Only) data is updated before entering this function 00053 * and can be read to obtain information for supervision or control 00054 * 00055 * WO_(Write Only) data is written to the LL processor after 00056 * execution of this function. 00057 * 00058 * WO_ and RO_ structs are defined in sdk.h 00059 * 00060 * The struct RO_ALL_Data (defined in sdk.h) 00061 * is used to read all sensor data, results of the data fusion 00062 * and R/C inputs transmitted from the LL-processor. This struct is 00063 * automatically updated at 1 kHz. 00064 * */ 00065 00066 /* How to flash the high level processor 00067 * 00068 * The easiest way to get your code on the high level processor is to use the JTAG-adapter. 00069 * 00070 * It needs three steps to get your code on the high level processor. 00071 * 1. Build your code ("Build Project") 00072 * 2. Connect your JTAG adapter and start the JTAG connection ("OpenOCD Asctec-JTAG") 00073 * 3. Flash the processor ("Asctec JTAG Debug") 00074 * 00075 * In the menu "Run -> External Tools -> External Tools Configuration..." you 00076 * will find "OpenOCD Asctec-JTAG". If the JTAG connection was activated 00077 * correctly, the console will show only the following line: 00078 * "Info: openocd.c:92 main(): Open On-Chip Debugger (2007-08-10 22:30 CEST)" 00079 * 00080 * Do not launch more than ONE JTAG-connection at the same time! 00081 * 00082 * In the menu "Run -> Debug Configurations..." you will find "Asctec JTAG Debug" 00083 * If the code was successfully flashed on the processor, the program will switch 00084 * to the Debug window. 00085 ************* 00086 * If you want to flash the high level processor using a serial interface (AscTec USB adapter) 00087 * and bootloader software like "Flash Magic", you need to change 00088 * the following in the "makefile" (line 113): 00089 * 00090 * FORMAT = ihex 00091 * #FORMAT = binary 00092 * 00093 * After buidling your code you will find the main.hex in your workspace folder. 00094 ************* 00095 * */ 00096 00097 /* After flashing the HL, your code can be debugged online. The ARM7 supports ONE hardware breakpoint. 00098 * You can monitor the CPU-load by looking at HL_Status.cpu_load. As long as this value is 1000 your 00099 * code in SDK_mainloop() is executed at 1 kHz. 00100 * 00101 * To activate the SDK controls, the serial interface switch on your R/C (channel 5) needs to be in ON position. 00102 * 00103 * If your project needs communication over a serial link, you find an example of how to do so in main.c, line 226. 00104 * All sample functions for transmitting and receiving data over the UART (HL_serial_0) are in uart.c - you can use 00105 * these examples to code your own communication state machine. 00106 */ 00107 00108 00109 void SDK_mainloop(void) //write your own code within this function 00110 { 00111 00112 00113 //you can select an example by using ONE of the functions below. 00114 //CAUTION! Read the code of the examples before you test them on your UAV! 00115 00116 //SDK_EXAMPLE_direct_individual_motor_commands(); 00117 //SDK_EXAMPLE_direct_motor_commands_with_standard_output_mapping(); 00118 //SDK_EXAMPLE_attitude_commands(); 00119 //SDK_EXAMPLE_gps_waypoint_control(); 00120 00121 //example to turn motors on and off every 2 seconds 00122 /* 00123 static int timer=0; 00124 if(++timer<1000) SDK_EXAMPLE_turn_motors_on(); 00125 else if(timer<2000) SDK_EXAMPLE_turn_motors_off(); 00126 else timer=0; 00127 */ 00128 } 00129 00130 /* the following example shows the direct motor command usage by mapping the stick directly to the motor outputs (do NOT try to fly ;-) ) 00131 * 00132 */ 00133 void SDK_EXAMPLE_direct_individual_motor_commands(void) 00134 { 00135 00136 WO_SDK.ctrl_mode=0x00; //0x00: direct individual motor control: individual commands for motors 0..3 00137 //0x01: direct motor control using standard output mapping: commands are interpreted as pitch, roll, yaw and thrust inputs; no attitude controller active 00138 //0x02: attitude and throttle control: commands are input for standard attitude controller 00139 //0x03: GPS waypoint control 00140 00141 WO_SDK.ctrl_enabled=1; //0: disable control by HL processor 00142 //1: enable control by HL processor 00143 00144 WO_SDK.disable_motor_onoff_by_stick=0; 00145 00146 unsigned int i; 00147 00148 //scale throttle stick to [0..200] and map it to all motors 00149 WO_Direct_Individual_Motor_Control.motor[0]=RO_ALL_Data.channel[2]/21; 00150 WO_Direct_Individual_Motor_Control.motor[1]=RO_ALL_Data.channel[2]/21; 00151 WO_Direct_Individual_Motor_Control.motor[2]=RO_ALL_Data.channel[2]/21; 00152 WO_Direct_Individual_Motor_Control.motor[3]=RO_ALL_Data.channel[2]/21; 00153 WO_Direct_Individual_Motor_Control.motor[4]=RO_ALL_Data.channel[2]/21; 00154 WO_Direct_Individual_Motor_Control.motor[5]=RO_ALL_Data.channel[2]/21; 00155 00156 //make sure commands are never 0 so that motors will always keep spinning 00157 //also make sure that commands stay within range 00158 for(i=0;i<6;i++) 00159 { 00160 if(!WO_Direct_Individual_Motor_Control.motor[i]) WO_Direct_Individual_Motor_Control.motor[i]=1; 00161 else if (WO_Direct_Individual_Motor_Control.motor[i]>200) WO_Direct_Individual_Motor_Control.motor[i]=200; 00162 } 00163 } 00164 00165 00166 void SDK_EXAMPLE_direct_motor_commands_with_standard_output_mapping(void) 00167 { 00168 WO_SDK.ctrl_mode=0x01; //0x00: direct individual motor control: individual commands for motors 0..3 00169 //0x01: direct motor control using standard output mapping: commands are interpreted as pitch, roll, yaw and thrust inputs; no attitude controller active 00170 //0x02: attitude and throttle control: commands are input for standard attitude controller 00171 //0x03: GPS waypoint control 00172 00173 WO_SDK.ctrl_enabled=1; //0: disable control by HL processor 00174 //1: enable control by HL processor 00175 00176 /* 00177 * Stick commands directly mapped to motors, NO attitude control! Do NOT try to fly! 00178 * */ 00179 00180 WO_Direct_Motor_Control.pitch=(4095-RO_ALL_Data.channel[0])/21; 00181 WO_Direct_Motor_Control.roll=RO_ALL_Data.channel[1]/21; 00182 WO_Direct_Motor_Control.thrust=RO_ALL_Data.channel[2]/21; 00183 WO_Direct_Motor_Control.yaw=(4095-RO_ALL_Data.channel[3])/21; 00184 00185 } 00186 00187 00188 void SDK_EXAMPLE_attitude_commands(void) 00189 { 00190 WO_SDK.ctrl_mode=0x02; //0x00: direct individual motor control: individual commands for motors 0..3 00191 //0x01: direct motor control using standard output mapping: commands are interpreted as pitch, roll, yaw and thrust inputs; no attitude controller active 00192 //0x02: attitude and throttle control: commands are input for standard attitude controller 00193 //0x03: GPS waypoint control 00194 00195 WO_SDK.ctrl_enabled=1; //0: disable control by HL processor 00196 //1: enable control by HL processor 00197 00198 //with this example the UAV will go to ~10% throttle when SDK control is activated 00199 WO_CTRL_Input.ctrl=0x08; //0x08: enable throttle control by HL. Height control and GPS are deactivated!! 00200 //pitch, roll and yaw are still commanded via the remote control 00201 00202 WO_CTRL_Input.thrust=400; //10% throttle command 00203 00204 00205 } 00206 00207 00208 00209 /* This function demonstrates a simple waypoint command generation. Switch on Channel 7 is used 00210 * to activate a 15m by 15m square. Therefore a waypoint is calculated from the current position and 00211 * height and is transmitted to the low level processor. The waypoint status is monitored to switch to 00212 * the next waypoint after the current one is reached. 00213 * 00214 * wpCtrlWpCmd is used to send a command to the low level processor. Different options like waypoint, launch, land, come home, set home 00215 * are available. See LL_HL_comm.h for WP_CMD_* defines 00216 * 00217 * wpCtrlWpCmdUpdated has to be set to 1 to send the command. When the cmd is sent it is set back to 0 automatically 00218 * 00219 * wpCtrlAckTrigger is set to 1 when the LL accepts the waypoint 00220 * 00221 * wpCtrlNavStatus gives you a navigation status. See WP_NAVSTAT_* defines in SDK.h for options 00222 * 00223 * wpCtrlDistToWp gives you the current distance to the current waypoint in dm (= 10 cm) 00224 */ 00225 void SDK_EXAMPLE_gps_waypoint_control() 00226 { 00227 static unsigned char wpExampleState=0; 00228 static double originLat,originLon; 00229 00230 00231 WO_SDK.ctrl_mode=0x03; 00232 00233 WO_SDK.ctrl_enabled=1; //0: disable control by HL processor 00234 //1: enable control by HL processor 00235 00236 switch (wpExampleState) 00237 { 00238 //prior to start, the lever on channel 7 has to be in "OFF" position 00239 case 0: 00240 if (RO_RC_Data.channel[6]<1600) 00241 wpExampleState=1; 00242 break; 00243 00244 case 1: 00245 if (RO_RC_Data.channel[6]>2400) 00246 { 00247 double lat,lon; 00248 //lever was set to "ON" state -> calculate and send first waypoint and switch state 00249 00250 //fill waypoint structure 00251 wpToLL.max_speed=100; 00252 wpToLL.pos_acc=3000; //3m accuracy 00253 wpToLL.time=400; //4 seconds waiting time at each waypoint 00254 wpToLL.wp_activated=1; 00255 00256 //see LL_HL_comm.h for WPPROP defines 00257 wpToLL.properties=WPPROP_ABSCOORDS|WPPROP_AUTOMATICGOTO|WPPROP_HEIGHTENABLED|WPPROP_YAWENABLED; 00258 00259 //use current height and yaw 00260 wpToLL.yaw=IMU_CalcData.angle_yaw; //use current yaw 00261 wpToLL.height=IMU_CalcData.height; //use current height 00262 00263 originLat=(double)GPS_Data.latitude/10000000.0; 00264 originLon=(double)GPS_Data.longitude/10000000.0; 00265 00266 //calculate a position 15m north of us 00267 xy2latlon(originLat,originLon,0.0,15.0,&lat,&lon); 00268 00269 wpToLL.X=lon*10000000; 00270 wpToLL.Y=lat*10000000; 00271 00272 //calc chksum 00273 wpToLL.chksum = 0xAAAA 00274 + wpToLL.yaw 00275 + wpToLL.height 00276 + wpToLL.time 00277 + wpToLL.X 00278 + wpToLL.Y 00279 + wpToLL.max_speed 00280 + wpToLL.pos_acc 00281 + wpToLL.properties 00282 + wpToLL.wp_activated; 00283 00284 //send waypoint 00285 wpCtrlAckTrigger=0; 00286 wpCtrlWpCmd=WP_CMD_SINGLE_WP; 00287 wpCtrlWpCmdUpdated=1; 00288 00289 wpExampleState=2; 00290 00291 } 00292 break; 00293 00294 case 2: 00295 //wait until cmd is processed and sent to LL processor 00296 if ((wpCtrlWpCmdUpdated==0) && (wpCtrlAckTrigger)) 00297 { 00298 //check if waypoint was reached and wait time is over 00299 if (wpCtrlNavStatus&(WP_NAVSTAT_REACHED_POS_TIME)) 00300 { 00301 //new waypoint 00302 double lat,lon; 00303 00304 //fill waypoint structure 00305 wpToLL.max_speed=100; 00306 wpToLL.pos_acc=3000; //3m accuracy 00307 wpToLL.time=400; //4 seconds wait time 00308 wpToLL.wp_activated=1; 00309 00310 //see LL_HL_comm.h for WPPROP defines 00311 wpToLL.properties=WPPROP_ABSCOORDS|WPPROP_AUTOMATICGOTO|WPPROP_HEIGHTENABLED|WPPROP_YAWENABLED; 00312 00313 //use current height and yaw 00314 wpToLL.yaw=IMU_CalcData.angle_yaw; //use current yaw 00315 wpToLL.height=IMU_CalcData.height; //use current height 00316 00317 //calculate a position 15m north and 15m east of origin 00318 xy2latlon(originLat,originLon,15.0,15.0,&lat,&lon); 00319 00320 wpToLL.X=lon*10000000; 00321 wpToLL.Y=lat*10000000; 00322 00323 //calc chksum 00324 wpToLL.chksum = 0xAAAA 00325 + wpToLL.yaw 00326 + wpToLL.height 00327 + wpToLL.time 00328 + wpToLL.X 00329 + wpToLL.Y 00330 + wpToLL.max_speed 00331 + wpToLL.pos_acc 00332 + wpToLL.properties 00333 + wpToLL.wp_activated; 00334 //send waypoint 00335 wpCtrlAckTrigger=0; 00336 wpCtrlWpCmd=WP_CMD_SINGLE_WP; 00337 wpCtrlWpCmdUpdated=1; 00338 00339 wpExampleState=3; 00340 } 00341 00342 if (wpCtrlNavStatus&WP_NAVSTAT_PILOT_ABORT) 00343 wpExampleState=0; 00344 00345 00346 } 00347 if (RO_RC_Data.channel[6]<1600) 00348 wpExampleState=0; 00349 break; 00350 00351 case 3: 00352 //wait until cmd is processed and sent to LL processor 00353 if ((wpCtrlWpCmdUpdated==0) && (wpCtrlAckTrigger)) 00354 { 00355 //check if waypoint was reached and wait time is over 00356 if (wpCtrlNavStatus&(WP_NAVSTAT_REACHED_POS_TIME)) 00357 { 00358 //new waypoint 00359 double lat,lon; 00360 00361 //fill waypoint structure 00362 wpToLL.max_speed=100; 00363 wpToLL.pos_acc=3000; //3m accuracy 00364 wpToLL.time=400; //4 seconds wait time 00365 wpToLL.wp_activated=1; 00366 00367 //see LL_HL_comm.h for WPPROP defines 00368 wpToLL.properties=WPPROP_ABSCOORDS|WPPROP_AUTOMATICGOTO|WPPROP_HEIGHTENABLED|WPPROP_YAWENABLED; 00369 00370 //use current height and yaw 00371 wpToLL.yaw=IMU_CalcData.angle_yaw; //use current yaw 00372 wpToLL.height=IMU_CalcData.height; //use current height 00373 00374 //calculate a position 15m east of origin 00375 xy2latlon(originLat,originLon,15.0,0.0,&lat,&lon); 00376 00377 wpToLL.X=lon*10000000; 00378 wpToLL.Y=lat*10000000; 00379 00380 //calc chksum 00381 wpToLL.chksum = 0xAAAA 00382 + wpToLL.yaw 00383 + wpToLL.height 00384 + wpToLL.time 00385 + wpToLL.X 00386 + wpToLL.Y 00387 + wpToLL.max_speed 00388 + wpToLL.pos_acc 00389 + wpToLL.properties 00390 + wpToLL.wp_activated; 00391 00392 //send waypoint 00393 wpCtrlAckTrigger=0; 00394 wpCtrlWpCmd=WP_CMD_SINGLE_WP; 00395 wpCtrlWpCmdUpdated=1; 00396 00397 wpExampleState=4; 00398 } 00399 00400 if (wpCtrlNavStatus&WP_NAVSTAT_PILOT_ABORT) 00401 wpExampleState=0; 00402 00403 00404 } 00405 if (RO_RC_Data.channel[6]<1600) 00406 wpExampleState=0; 00407 break; 00408 00409 case 4: 00410 //wait until cmd is processed and sent to LL processor 00411 if ((wpCtrlWpCmdUpdated==0) && (wpCtrlAckTrigger)) 00412 { 00413 //check if waypoint was reached and wait time is over 00414 if (wpCtrlNavStatus&(WP_NAVSTAT_REACHED_POS_TIME)) 00415 { 00416 00417 //fill waypoint structure 00418 wpToLL.max_speed=100; 00419 wpToLL.pos_acc=3000; //3m accuracy 00420 wpToLL.time=400; //4 seconds wait time 00421 wpToLL.wp_activated=1; 00422 00423 //see LL_HL_comm.h for WPPROP defines 00424 wpToLL.properties=WPPROP_ABSCOORDS|WPPROP_AUTOMATICGOTO|WPPROP_HEIGHTENABLED|WPPROP_YAWENABLED; 00425 00426 //use current height and yaw 00427 wpToLL.yaw=IMU_CalcData.angle_yaw; //use current yaw 00428 wpToLL.height=IMU_CalcData.height; //use current height 00429 00430 //go to the beginning 00431 00432 wpToLL.X=originLon*10000000; 00433 wpToLL.Y=originLat*10000000; 00434 00435 //calc chksum 00436 wpToLL.chksum = 0xAAAA 00437 + wpToLL.yaw 00438 + wpToLL.height 00439 + wpToLL.time 00440 + wpToLL.X 00441 + wpToLL.Y 00442 + wpToLL.max_speed 00443 + wpToLL.pos_acc 00444 + wpToLL.properties 00445 + wpToLL.wp_activated; 00446 00447 //send waypoint 00448 wpCtrlAckTrigger=0; 00449 wpCtrlWpCmd=WP_CMD_SINGLE_WP; 00450 wpCtrlWpCmdUpdated=1; 00451 00452 wpExampleState=0; 00453 } 00454 00455 if (wpCtrlNavStatus&WP_NAVSTAT_PILOT_ABORT) 00456 wpExampleState=0; 00457 00458 00459 } 00460 if (RO_RC_Data.channel[6]<1600) 00461 wpExampleState=0; 00462 break; 00463 00464 default: 00465 wpExampleState=0; 00466 break; 00467 } 00468 00469 } 00470 00471 int SDK_EXAMPLE_turn_motors_on(void) //hold throttle stick down and yaw stick fully left to turn motors on 00472 { 00473 static int timeout=0; 00474 00475 WO_SDK.ctrl_mode=0x02; //0x00: direct individual motor control: individual commands for motors 0..3 00476 //0x01: direct motor control using standard output mapping: commands are interpreted as pitch, roll, yaw and thrust inputs; no attitude controller active 00477 //0x02: attitude and throttle control: commands are input for standard attitude controller 00478 //0x03: GPS waypoint control 00479 00480 WO_SDK.ctrl_enabled=1; //0: disable control by HL processor 00481 //1: enable control by HL processor 00482 00483 WO_SDK.disable_motor_onoff_by_stick=0; //make sure stick command is accepted 00484 00485 if(++timeout>=1000) 00486 { 00487 timeout=0; 00488 return(1); //1 => start sequence completed => motors running => user can stop calling this function 00489 } 00490 else if(timeout>500) //neutral stick command for 500 ms 00491 { 00492 WO_CTRL_Input.ctrl=0x0C; //0x0C: enable throttle control and yaw control 00493 WO_CTRL_Input.thrust=0; //use R/C throttle stick input /2 to control thrust (just for testing) 00494 WO_CTRL_Input.yaw=0; 00495 return(0); 00496 } 00497 else //hold stick command for 500 ms 00498 { 00499 WO_CTRL_Input.ctrl=0x0C; //0x0C: enable throttle control and yaw control 00500 WO_CTRL_Input.thrust=0; //use R/C throttle stick input /2 to control thrust (just for testing) 00501 WO_CTRL_Input.yaw=-2047; 00502 return(0); 00503 } 00504 00505 } 00506 00507 int SDK_EXAMPLE_turn_motors_off(void) //hold throttle stick down and yaw stick fully right to turn motors off 00508 { 00509 static int timeout=0; 00510 00511 WO_SDK.ctrl_mode=0x02; //0x00: direct individual motor control: individual commands for motors 0..3 00512 //0x01: direct motor control using standard output mapping: commands are interpreted as pitch, roll, yaw and thrust inputs; no attitude controller active 00513 //0x02: attitude and throttle control: commands are input for standard attitude controller 00514 //0x03: GPS waypoint control 00515 00516 WO_SDK.ctrl_enabled=1; //0: disable control by HL processor 00517 //1: enable control by HL processor 00518 00519 WO_SDK.disable_motor_onoff_by_stick=0; //make sure stick command is accepted 00520 00521 if(++timeout>=1000) 00522 { 00523 timeout=0; 00524 return(1); //1 => stop sequence completed => motors turned off => user can stop calling this function 00525 } 00526 else if(timeout>500) //neutral stick command for 500 ms 00527 { 00528 WO_CTRL_Input.ctrl=0x0C; //0x0C: enable throttle control and yaw control 00529 WO_CTRL_Input.thrust=0; //use R/C throttle stick input /2 to control thrust (just for testing) 00530 WO_CTRL_Input.yaw=0; 00531 return(0); 00532 } 00533 else //hold stick command for 500 ms 00534 { 00535 WO_CTRL_Input.ctrl=0x0C; //0x0C: enable throttle control and yaw control 00536 WO_CTRL_Input.thrust=0; //use R/C throttle stick input /2 to control thrust (just for testing) 00537 WO_CTRL_Input.yaw=2047; 00538 return(0); 00539 } 00540 } 00541 00542