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 #include "main.h" 00030 #include "LL_HL_comm.h" 00031 #include "system.h" 00032 #include "gpsmath.h" 00033 #include "sdk.h" 00034 #include "declination.h" 00035 00036 unsigned short SSP_ack=0; 00037 extern char SPIWRData[128]; 00038 extern char data_sent_to_HL; 00039 extern char data_sent_to_LL; 00040 extern unsigned int SPIWR_num_bytes; 00041 00042 struct LL_ATTITUDE_DATA LL_1khz_attitude_data; 00043 struct LL_CONTROL_INPUT LL_1khz_control_input; 00044 00045 unsigned char wpCtrlWpCmd=0; 00046 unsigned char wpCtrlWpCmdUpdated=0; 00047 00048 unsigned char wpCtrlAckTrigger=0; 00049 00050 unsigned short wpCtrlNavStatus=0; 00051 unsigned short wpCtrlDistToWp=0; 00052 00053 struct WAYPOINT wpToLL; 00054 00055 void SSP_data_distribution_HL(void) 00056 { 00057 unsigned char i; 00058 unsigned char current_page=LL_1khz_attitude_data.system_flags&0x03; 00059 00060 if(LL_1khz_attitude_data.system_flags&SF_GPS_NEW) gpsDataOkTrigger=0; 00061 00062 IMU_CalcData.angle_nick=LL_1khz_attitude_data.angle_pitch*10; 00063 IMU_CalcData.angle_roll=LL_1khz_attitude_data.angle_roll*10; 00064 IMU_CalcData.angle_yaw=LL_1khz_attitude_data.angle_yaw*10; 00065 00066 IMU_CalcData.angvel_nick=LL_1khz_attitude_data.angvel_pitch; 00067 IMU_CalcData.angvel_roll=LL_1khz_attitude_data.angvel_roll; 00068 IMU_CalcData.angvel_yaw=LL_1khz_attitude_data.angvel_yaw; 00069 00070 RO_ALL_Data.angle_pitch=IMU_CalcData.angle_nick; 00071 RO_ALL_Data.angle_roll=IMU_CalcData.angle_roll; 00072 RO_ALL_Data.angle_yaw=IMU_CalcData.angle_yaw; 00073 00074 RO_ALL_Data.angvel_pitch=LL_1khz_attitude_data.angvel_pitch; 00075 RO_ALL_Data.angvel_roll=LL_1khz_attitude_data.angvel_roll; 00076 RO_ALL_Data.angvel_yaw=LL_1khz_attitude_data.angvel_yaw; 00077 00078 if(!current_page) //page 0 00079 { 00080 for(i=0;i<8;i++) 00081 { 00082 RO_RC_Data.channel[i]=LL_1khz_attitude_data.RC_data[i]*16; 00083 RO_ALL_Data.channel[i]=RO_RC_Data.channel[i]; 00084 } 00085 IMU_CalcData.acc_x_calib=LL_1khz_attitude_data.acc_x*10; 00086 IMU_CalcData.acc_y_calib=LL_1khz_attitude_data.acc_y*10; 00087 IMU_CalcData.acc_z_calib=LL_1khz_attitude_data.acc_z*10; 00088 00089 //system is initialized as soon as values differ from 0 00090 if(IMU_CalcData.acc_z_calib&&(SYSTEM_initialized<10)) SYSTEM_initialized++; 00091 00092 RO_ALL_Data.acc_x=LL_1khz_attitude_data.acc_x*10; 00093 RO_ALL_Data.acc_y=LL_1khz_attitude_data.acc_y*10; 00094 RO_ALL_Data.acc_z=LL_1khz_attitude_data.acc_z*10; 00095 00096 RO_ALL_Data.fusion_latitude=LL_1khz_attitude_data.latitude_best_estimate; 00097 RO_ALL_Data.fusion_longitude=LL_1khz_attitude_data.longitude_best_estimate; 00098 00099 } 00100 else if(current_page==1) //page 1 00101 { 00102 IMU_CalcData.height=LL_1khz_attitude_data.height; 00103 IMU_CalcData.dheight=LL_1khz_attitude_data.dheight; 00104 00105 RO_ALL_Data.fusion_height=LL_1khz_attitude_data.height; 00106 RO_ALL_Data.fusion_dheight=LL_1khz_attitude_data.dheight; 00107 00108 RO_ALL_Data.fusion_speed_x=LL_1khz_attitude_data.speed_x_best_estimate; 00109 RO_ALL_Data.fusion_speed_y=LL_1khz_attitude_data.speed_y_best_estimate; 00110 for(i=0;i<6;i++) 00111 { 00112 RO_ALL_Data.motor_rpm[i]=LL_1khz_attitude_data.motor_data[i]; 00113 } 00114 } 00115 else if(current_page==2) 00116 { 00117 IMU_CalcData.Hx=LL_1khz_attitude_data.mag_x; 00118 IMU_CalcData.Hy=LL_1khz_attitude_data.mag_y; 00119 IMU_CalcData.Hz=LL_1khz_attitude_data.mag_z; 00120 00121 RO_ALL_Data.Hx=LL_1khz_attitude_data.mag_x; 00122 RO_ALL_Data.Hy=LL_1khz_attitude_data.mag_y; 00123 RO_ALL_Data.Hz=LL_1khz_attitude_data.mag_z; 00124 00125 unsigned char slowDataUpChannelSelect=(LL_1khz_attitude_data.status2>>1)&0x7F; 00126 switch (slowDataUpChannelSelect) 00127 { 00128 case SUDC_FLIGHTTIME: 00129 00130 HL_Status.flight_time=LL_1khz_attitude_data.slowDataUpChannelDataShort; 00131 break; 00132 case SUDC_NAVSTATUS: 00133 wpCtrlNavStatus=LL_1khz_attitude_data.slowDataUpChannelDataShort; 00134 break; 00135 case SUDC_DISTTOWP: 00136 wpCtrlDistToWp=LL_1khz_attitude_data.slowDataUpChannelDataShort; 00137 break; 00138 case SUDC_WPACKTRIGGER: 00139 wpCtrlAckTrigger=LL_1khz_attitude_data.slowDataUpChannelDataShort; 00140 break; 00141 00142 } 00143 00144 } 00145 } 00146 00147 int HL2LL_write_cycle(void) //write data to low-level processor 00148 { 00149 static char pageselect=0; 00150 unsigned char i; 00151 00152 if(!data_sent_to_LL) return(0); 00153 00154 //update 1kHz data 00155 LL_1khz_control_input.system_flags=0|pageselect; 00156 //SSP_ack=0; //reset ack 00157 00158 if(gpsDataOkTrigger) LL_1khz_control_input.system_flags|=SF_GPS_NEW; 00159 00160 if(WO_SDK.ctrl_enabled) LL_1khz_control_input.system_flags|=SF_HL_CONTROL_ENABLED|SF_NEW_SDK; 00161 else LL_1khz_control_input.system_flags&=~(SF_HL_CONTROL_ENABLED|SF_NEW_SDK); 00162 00163 if(WO_SDK.disable_motor_onoff_by_stick) LL_1khz_control_input.system_flags|=SF_SDK_DISABLE_MOTORONOFF_BY_STICK; 00164 else LL_1khz_control_input.system_flags&=~SF_SDK_DISABLE_MOTORONOFF_BY_STICK; 00165 00166 if(WO_SDK.ctrl_mode==0x00) //direct individual motor control 00167 { 00168 LL_1khz_control_input.system_flags|=SF_DIRECT_MOTOR_CONTROL_INDIVIDUAL; 00169 for(i=0;i<8;i++) 00170 { 00171 LL_1khz_control_input.direct_motor_control[i]=WO_Direct_Individual_Motor_Control.motor[i]; 00172 } 00173 } 00174 else if(WO_SDK.ctrl_mode==0x01) //direct motor control with standard output mapping 00175 { 00176 LL_1khz_control_input.system_flags|=SF_DIRECT_MOTOR_CONTROL; 00177 LL_1khz_control_input.direct_motor_control[0]=WO_Direct_Motor_Control.pitch; 00178 LL_1khz_control_input.direct_motor_control[1]=WO_Direct_Motor_Control.roll; 00179 LL_1khz_control_input.direct_motor_control[2]=WO_Direct_Motor_Control.yaw; 00180 LL_1khz_control_input.direct_motor_control[3]=WO_Direct_Motor_Control.thrust; 00181 } 00182 else if (WO_SDK.ctrl_mode==0x02) //attitude control 00183 { 00184 LL_1khz_control_input.system_flags&=~(SF_DIRECT_MOTOR_CONTROL|SF_DIRECT_MOTOR_CONTROL_INDIVIDUAL|SF_WAYPOINT_MODE); //no additional system flag => attitude control is "standard mode" 00185 LL_1khz_control_input.ctrl_flags=WO_CTRL_Input.ctrl; 00186 LL_1khz_control_input.pitch=WO_CTRL_Input.pitch; 00187 LL_1khz_control_input.roll=WO_CTRL_Input.roll; 00188 LL_1khz_control_input.yaw=WO_CTRL_Input.yaw; 00189 LL_1khz_control_input.thrust=WO_CTRL_Input.thrust; 00190 } 00191 else if (WO_SDK.ctrl_mode==0x03) //gps waypoint control 00192 { 00193 LL_1khz_control_input.system_flags|=SF_WAYPOINT_MODE; 00194 00195 //check if new command should be send 00196 00197 if (wpCtrlWpCmdUpdated) 00198 { 00199 00200 if (wpCtrlWpCmd==WP_CMD_SINGLE_WP) 00201 { 00202 if (wpCtrlWpCmdUpdated==1) 00203 { 00204 LL_1khz_control_input.ctrl_flags&=0x00FF; 00205 LL_1khz_control_input.ctrl_flags|=WP_CMD_SINGLE_WP_PART1<<8; 00206 00207 LL_1khz_control_input.pitch=wpToLL.X&0xFFFF; 00208 LL_1khz_control_input.roll=wpToLL.X>>16; 00209 LL_1khz_control_input.thrust=wpToLL.Y&0xFFFF; 00210 LL_1khz_control_input.yaw=wpToLL.Y>>16; 00211 LL_1khz_control_input.direct_motor_control[0]=wpToLL.height; 00212 LL_1khz_control_input.direct_motor_control[1]=wpToLL.height>>8; 00213 LL_1khz_control_input.direct_motor_control[2]=wpToLL.height>>16; 00214 LL_1khz_control_input.direct_motor_control[3]=wpToLL.height>>24; 00215 LL_1khz_control_input.direct_motor_control[4]=wpToLL.yaw; 00216 LL_1khz_control_input.direct_motor_control[5]=wpToLL.yaw>>8; 00217 LL_1khz_control_input.direct_motor_control[6]=wpToLL.yaw>>16; 00218 LL_1khz_control_input.direct_motor_control[7]=wpToLL.yaw>>24; 00219 00220 wpCtrlWpCmdUpdated++; 00221 }else if (wpCtrlWpCmdUpdated==2) 00222 { 00223 LL_1khz_control_input.ctrl_flags&=0x00FF; 00224 LL_1khz_control_input.ctrl_flags|=WP_CMD_SINGLE_WP_PART2<<8; 00225 00226 LL_1khz_control_input.pitch=wpToLL.time; 00227 LL_1khz_control_input.roll=0; //wpToLL.cam_angle_pitch; 00228 LL_1khz_control_input.thrust=wpToLL.pos_acc; 00229 LL_1khz_control_input.yaw=wpToLL.chksum; 00230 LL_1khz_control_input.direct_motor_control[0]=0; //wpToLL.cam_angle_roll; 00231 LL_1khz_control_input.direct_motor_control[1]=wpToLL.max_speed; 00232 LL_1khz_control_input.direct_motor_control[2]=wpToLL.properties; 00233 LL_1khz_control_input.direct_motor_control[3]=wpToLL.wp_activated; 00234 LL_1khz_control_input.direct_motor_control[4]=0; 00235 LL_1khz_control_input.direct_motor_control[5]=0; 00236 LL_1khz_control_input.direct_motor_control[6]=0; 00237 LL_1khz_control_input.direct_motor_control[7]=0; 00238 wpCtrlWpCmdUpdated=0; 00239 wpCtrlNavStatus=0; 00240 } 00241 }else 00242 { 00243 LL_1khz_control_input.ctrl_flags&=0x00FF; 00244 LL_1khz_control_input.ctrl_flags|=wpCtrlWpCmd<<8; 00245 wpCtrlWpCmdUpdated=0; 00246 } 00247 }else 00248 { 00249 LL_1khz_control_input.ctrl_flags&=0x00FF; 00250 } 00251 } 00252 else LL_1khz_control_input.system_flags&=~(SF_DIRECT_MOTOR_CONTROL|SF_DIRECT_MOTOR_CONTROL_INDIVIDUAL|SF_WAYPOINT_MODE); 00253 00254 if(pageselect==0) 00255 { 00256 //fill struct with 500Hz data 00257 LL_1khz_control_input.latitude=GPS_Data.latitude; 00258 LL_1khz_control_input.longitude=GPS_Data.longitude; 00259 LL_1khz_control_input.height=GPS_Data.height; 00260 LL_1khz_control_input.speed_x=GPS_Data.speed_x; 00261 LL_1khz_control_input.speed_y=GPS_Data.speed_y; 00262 LL_1khz_control_input.heading=GPS_Data.heading; 00263 LL_1khz_control_input.status=GPS_Data.status; 00264 00265 //write data 00266 LL_write_ctrl_data(pageselect); 00267 //set pageselect to other page for next cycle 00268 pageselect=1; 00269 } 00270 else //pageselect=1 00271 { 00272 //fill struct with 500Hz data 00273 LL_1khz_control_input.hor_accuracy=GPS_Data.horizontal_accuracy; 00274 LL_1khz_control_input.vert_accuracy=GPS_Data.vertical_accuracy; 00275 LL_1khz_control_input.speed_accuracy=GPS_Data.speed_accuracy; 00276 LL_1khz_control_input.numSV=GPS_Data.numSV; 00277 LL_1khz_control_input.battery_voltage_1=HL_Status.battery_voltage_1; 00278 LL_1khz_control_input.battery_voltage_2=HL_Status.battery_voltage_2; 00279 if (declinationAvailable==1) 00280 { 00281 declinationAvailable=2; 00282 LL_1khz_control_input.slowDataChannelSelect=SDC_DECLINATION; 00283 LL_1khz_control_input.slowDataChannelDataShort=estimatedDeclination; 00284 00285 } 00286 else if (declinationAvailable==2) 00287 { 00288 declinationAvailable=3; 00289 LL_1khz_control_input.slowDataChannelSelect=SDC_INCLINATION; 00290 LL_1khz_control_input.slowDataChannelDataShort=estimatedInclination; 00291 00292 } 00293 else 00294 { 00295 LL_1khz_control_input.slowDataChannelDataShort=0; 00296 LL_1khz_control_input.slowDataChannelSelect=0; 00297 LL_1khz_control_input.slowDataChannelDataChar=0; 00298 } 00299 00300 00301 //write data 00302 LL_write_ctrl_data(pageselect); 00303 //set pageselect to other page for next cycle 00304 pageselect=0; 00305 } 00306 return(1); 00307 } 00308 00309 void LL_write_ctrl_data(char page) 00310 { 00311 unsigned int i; 00312 unsigned char *dataptr; 00313 static volatile short spi_chksum; 00314 00315 dataptr=(unsigned char *)&LL_1khz_control_input; 00316 00317 //initialize syncbytes 00318 SPIWRData[0]='>'; 00319 SPIWRData[1]='*'; 00320 00321 spi_chksum=0xAAAA; 00322 00323 if(!page) 00324 { 00325 for(i=2;i<40;i++) 00326 { 00327 SPIWRData[i]=*dataptr++; 00328 spi_chksum+=SPIWRData[i]; 00329 } 00330 } 00331 else 00332 { 00333 for(i=2;i<22;i++) 00334 { 00335 SPIWRData[i]=*dataptr++; 00336 spi_chksum+=SPIWRData[i]; 00337 } 00338 dataptr+=18; 00339 for(i=22;i<40;i++) 00340 { 00341 SPIWRData[i]=*dataptr++; 00342 spi_chksum+=SPIWRData[i]; 00343 } 00344 } 00345 00346 SPIWRData[40]=spi_chksum; //chksum LSB 00347 SPIWRData[41]=(spi_chksum>>8); //chksum MSB 00348 00349 SPIWR_num_bytes=42; 00350 data_sent_to_LL=0; 00351 } 00352 00353 00354 inline void SSP_rx_handler_HL(unsigned char SPI_rxdata) //rx_handler @ high-level processor 00355 { 00356 static volatile unsigned char SPI_syncstate=0; 00357 static volatile unsigned char SPI_rxcount=0; 00358 static volatile unsigned char *SPI_rxptr; 00359 static volatile unsigned char incoming_page; 00360 00361 //receive handler 00362 if (SPI_syncstate==0) 00363 { 00364 if (SPI_rxdata=='>') SPI_syncstate++; else SPI_syncstate=0; 00365 } 00366 else if (SPI_syncstate==1) 00367 { 00368 if (SPI_rxdata=='*') 00369 { 00370 SPI_syncstate++; 00371 SPI_rxptr=(unsigned char *)&LL_1khz_attitude_data; 00372 SPI_rxcount=40; 00373 } 00374 else SPI_syncstate=0; 00375 } 00376 else if (SPI_syncstate==2) 00377 { 00378 if(SPI_rxcount==26) //14 bytes transmitted => select 500Hz page 00379 { 00380 incoming_page=LL_1khz_attitude_data.system_flags&0x03; //system flags were already received 00381 if(incoming_page==1) SPI_rxptr+=26; 00382 else if(incoming_page==2) SPI_rxptr+=52; 00383 } 00384 SPI_rxcount--; 00385 *SPI_rxptr=SPI_rxdata; 00386 SPI_rxptr++; 00387 if (SPI_rxcount==0) 00388 { 00389 SPI_syncstate++; 00390 } 00391 } 00392 else if (SPI_syncstate==3) 00393 { 00394 if(SPI_rxdata=='<') //last byte ok => data should be valid 00395 { 00396 SSP_data_distribution_HL(); //only distribute data to other structs, if it was received correctly 00397 //ack data receiption 00398 } 00399 SPI_syncstate=0; 00400 } 00401 else SPI_syncstate=0; 00402 } 00403 00404 00405 00406 /* old file 00407 #include "main.h" 00408 #include "LL_HL_comm.h" 00409 #include "system.h" 00410 #include "gpsmath.h" 00411 #include "sdk.h" 00412 00413 unsigned short SSP_ack=0; 00414 extern char SPIWRData[128]; 00415 extern char data_sent_to_LL; 00416 extern unsigned int SPIWR_num_bytes; 00417 00418 struct LL_ATTITUDE_DATA LL_1khz_attitude_data; 00419 struct LL_CONTROL_INPUT LL_1khz_control_input; 00420 00421 void SSP_data_distribution_HL(void) 00422 { 00423 unsigned char i; 00424 unsigned char current_page=LL_1khz_attitude_data.system_flags&0x03; 00425 00426 if(LL_1khz_attitude_data.system_flags&SF_GPS_NEW) gpsDataOkTrigger=0; 00427 00428 IMU_CalcData.angle_nick=LL_1khz_attitude_data.angle_pitch*10; 00429 IMU_CalcData.angle_roll=LL_1khz_attitude_data.angle_roll*10; 00430 IMU_CalcData.angle_yaw=LL_1khz_attitude_data.angle_yaw*10; 00431 00432 IMU_CalcData.angvel_nick=LL_1khz_attitude_data.angvel_pitch; 00433 IMU_CalcData.angvel_roll=LL_1khz_attitude_data.angvel_roll; 00434 IMU_CalcData.angvel_yaw=LL_1khz_attitude_data.angvel_yaw; 00435 00436 if(!current_page) //page 0 00437 { 00438 for(i=0;i<8;i++) 00439 { 00440 RO_RC_Data.channel[i]=LL_1khz_attitude_data.RC_data[i]*16; 00441 } 00442 IMU_CalcData.acc_x_calib=LL_1khz_attitude_data.acc_x*10; 00443 IMU_CalcData.acc_y_calib=LL_1khz_attitude_data.acc_y*10; 00444 IMU_CalcData.acc_z_calib=LL_1khz_attitude_data.acc_z*10; 00445 } 00446 else if(current_page==1) //page 1 00447 { 00448 IMU_CalcData.height=LL_1khz_attitude_data.height; 00449 IMU_CalcData.dheight=LL_1khz_attitude_data.dheight; 00450 } 00451 else if(current_page==2) 00452 { 00453 IMU_CalcData.Hx=LL_1khz_attitude_data.mag_x; 00454 IMU_CalcData.Hy=LL_1khz_attitude_data.mag_y; 00455 IMU_CalcData.Hz=LL_1khz_attitude_data.mag_z; 00456 } 00457 } 00458 00459 int HL2LL_write_cycle(void) //write data to low-level processor 00460 { 00461 static char pageselect=0; 00462 00463 if(!data_sent_to_LL) return(0); 00464 00465 //update 1kHz data 00466 LL_1khz_control_input.system_flags=0|pageselect; 00467 //SSP_ack=0; //reset ack 00468 00469 if(gpsDataOkTrigger) LL_1khz_control_input.system_flags|=SF_GPS_NEW; 00470 00471 #ifndef FALCON 00472 if(WO_SDK.ctrl_enabled) LL_1khz_control_input.system_flags|=SF_HL_CONTROL_ENABLED; 00473 else LL_1khz_control_input.system_flags&=~SF_HL_CONTROL_ENABLED; 00474 00475 if(WO_SDK.ctrl_mode==0x01) LL_1khz_control_input.system_flags|=SF_DIRECT_MOTOR_CONTROL; 00476 else LL_1khz_control_input.system_flags&=~SF_DIRECT_MOTOR_CONTROL; 00477 #else //Disable Control Input if system is a FALCON 00478 LL_1khz_control_input.system_flags&=~SF_HL_CONTROL_ENABLED; 00479 LL_1khz_control_input.system_flags&=~SF_DIRECT_MOTOR_CONTROL; 00480 #endif 00481 00482 00483 00484 LL_1khz_control_input.ctrl_flags=WO_CTRL_Input.ctrl; 00485 LL_1khz_control_input.pitch=WO_CTRL_Input.pitch; 00486 LL_1khz_control_input.roll=WO_CTRL_Input.roll; 00487 LL_1khz_control_input.yaw=WO_CTRL_Input.yaw; 00488 LL_1khz_control_input.thrust=WO_CTRL_Input.thrust; 00489 00490 if(WO_SDK.ctrl_mode==0x01) 00491 { 00492 LL_1khz_control_input.direct_motor_control[0]=WO_Direct_Motor_Control.pitch; 00493 LL_1khz_control_input.direct_motor_control[1]=WO_Direct_Motor_Control.roll; 00494 LL_1khz_control_input.direct_motor_control[2]=WO_Direct_Motor_Control.yaw; 00495 LL_1khz_control_input.direct_motor_control[3]=WO_Direct_Motor_Control.thrust; 00496 } 00497 00498 // for(i=0;i<8;i++) 00499 // { 00500 // LL_1khz_control_input.direct_motor_control[i]=0; 00501 // } 00502 00503 if(pageselect==0) 00504 { 00505 //fill struct with 500Hz data 00506 LL_1khz_control_input.latitude=GPS_Data.latitude; 00507 LL_1khz_control_input.longitude=GPS_Data.longitude; 00508 LL_1khz_control_input.height=GPS_Data.height; 00509 LL_1khz_control_input.speed_x=GPS_Data.speed_x; 00510 LL_1khz_control_input.speed_y=GPS_Data.speed_y; 00511 LL_1khz_control_input.heading=GPS_Data.heading; 00512 LL_1khz_control_input.status=GPS_Data.status; 00513 00514 //write data 00515 LL_write_ctrl_data(pageselect); 00516 //set pageselect to other page for next cycle 00517 pageselect=1; 00518 } 00519 else //pageselect=1 00520 { 00521 //fill struct with 500Hz data 00522 LL_1khz_control_input.hor_accuracy=GPS_Data.horizontal_accuracy; 00523 LL_1khz_control_input.vert_accuracy=GPS_Data.vertical_accuracy; 00524 LL_1khz_control_input.speed_accuracy=GPS_Data.speed_accuracy; 00525 LL_1khz_control_input.numSV=GPS_Data.numSV; 00526 LL_1khz_control_input.battery_voltage_1=HL_Status.battery_voltage_1; 00527 LL_1khz_control_input.battery_voltage_2=HL_Status.battery_voltage_2; 00528 LL_1khz_control_input.dummy_500Hz_2=0; 00529 LL_1khz_control_input.dummy_500Hz_3=0; 00530 00531 //write data 00532 LL_write_ctrl_data(pageselect); 00533 //set pageselect to other page for next cycle 00534 pageselect=0; 00535 } 00536 return(1); 00537 } 00538 00539 void LL_write_ctrl_data(char page) 00540 { 00541 unsigned int i; 00542 unsigned char *dataptr; 00543 static volatile short spi_chksum; 00544 00545 dataptr=(unsigned char *)&LL_1khz_control_input; 00546 00547 //initialize syncbytes 00548 SPIWRData[0]='>'; 00549 SPIWRData[1]='*'; 00550 00551 spi_chksum=0xAAAA; 00552 00553 if(!page) 00554 { 00555 for(i=2;i<40;i++) 00556 { 00557 SPIWRData[i]=*dataptr++; 00558 spi_chksum+=SPIWRData[i]; 00559 } 00560 } 00561 else 00562 { 00563 for(i=2;i<22;i++) 00564 { 00565 SPIWRData[i]=*dataptr++; 00566 spi_chksum+=SPIWRData[i]; 00567 } 00568 dataptr+=18; 00569 for(i=22;i<40;i++) 00570 { 00571 SPIWRData[i]=*dataptr++; 00572 spi_chksum+=SPIWRData[i]; 00573 } 00574 } 00575 00576 SPIWRData[40]=spi_chksum; //chksum LSB 00577 SPIWRData[41]=(spi_chksum>>8); //chksum MSB 00578 00579 SPIWR_num_bytes=42; 00580 data_sent_to_LL=0; 00581 } 00582 00583 00584 inline void SSP_rx_handler_HL(unsigned char SPI_rxdata) //rx_handler @ high-level processor 00585 { 00586 static volatile unsigned char SPI_syncstate=0; 00587 static volatile unsigned char SPI_rxcount=0; 00588 static volatile unsigned char *SPI_rxptr; 00589 static volatile unsigned char incoming_page; 00590 00591 //receive handler 00592 if (SPI_syncstate==0) 00593 { 00594 if (SPI_rxdata=='>') SPI_syncstate++; else SPI_syncstate=0; 00595 } 00596 else if (SPI_syncstate==1) 00597 { 00598 if (SPI_rxdata=='*') 00599 { 00600 SPI_syncstate++; 00601 SPI_rxptr=(unsigned char *)&LL_1khz_attitude_data; 00602 SPI_rxcount=40; 00603 } 00604 else SPI_syncstate=0; 00605 } 00606 else if (SPI_syncstate==2) 00607 { 00608 if(SPI_rxcount==26) //14 bytes transmitted => select 500Hz page 00609 { 00610 incoming_page=LL_1khz_attitude_data.system_flags&0x03; //system flags were already received 00611 if(incoming_page==1) SPI_rxptr+=26; 00612 else if(incoming_page==2) SPI_rxptr+=52; 00613 } 00614 SPI_rxcount--; 00615 *SPI_rxptr=SPI_rxdata; 00616 SPI_rxptr++; 00617 if (SPI_rxcount==0) 00618 { 00619 SPI_syncstate++; 00620 } 00621 } 00622 else if (SPI_syncstate==3) 00623 { 00624 if(SPI_rxdata=='<') //last byte ok => data should be valid 00625 { 00626 SSP_data_distribution_HL(); //only distribute data to other structs, if it was received correctly 00627 //ack data receiption 00628 } 00629 SPI_syncstate=0; 00630 } 00631 else SPI_syncstate=0; 00632 } 00633 end old file */