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 00046 unsigned char wpCtrlWpCmd=0; 00047 unsigned char wpCtrlWpCmdUpdated=0; 00048 00049 unsigned char wpCtrlAckTrigger=0; 00050 00051 unsigned short wpCtrlNavStatus=0; 00052 unsigned short wpCtrlDistToWp=0; 00053 00054 struct WAYPOINT wpToLL; 00055 00056 void SSP_data_distribution_HL(void) 00057 { 00058 unsigned char i; 00059 unsigned char current_page=LL_1khz_attitude_data.system_flags&0x03; 00060 00061 if(LL_1khz_attitude_data.system_flags&SF_GPS_NEW) gpsDataOkTrigger=0; 00062 00063 IMU_CalcData.angle_nick=LL_1khz_attitude_data.angle_pitch*10; 00064 IMU_CalcData.angle_roll=LL_1khz_attitude_data.angle_roll*10; 00065 IMU_CalcData.angle_yaw=LL_1khz_attitude_data.angle_yaw*10; 00066 00067 IMU_CalcData.angvel_nick=LL_1khz_attitude_data.angvel_pitch; 00068 IMU_CalcData.angvel_roll=LL_1khz_attitude_data.angvel_roll; 00069 IMU_CalcData.angvel_yaw=LL_1khz_attitude_data.angvel_yaw; 00070 00071 RO_ALL_Data.angle_pitch=IMU_CalcData.angle_nick; 00072 RO_ALL_Data.angle_roll=IMU_CalcData.angle_roll; 00073 RO_ALL_Data.angle_yaw=IMU_CalcData.angle_yaw; 00074 00075 RO_ALL_Data.angvel_pitch=LL_1khz_attitude_data.angvel_pitch; 00076 RO_ALL_Data.angvel_roll=LL_1khz_attitude_data.angvel_roll; 00077 RO_ALL_Data.angvel_yaw=LL_1khz_attitude_data.angvel_yaw; 00078 00079 if(!current_page) //page 0 00080 { 00081 for(i=0;i<8;i++) 00082 { 00083 RO_RC_Data.channel[i]=LL_1khz_attitude_data.RC_data[i]*16; 00084 RO_ALL_Data.channel[i]=RO_RC_Data.channel[i]; 00085 } 00086 IMU_CalcData.acc_x_calib=LL_1khz_attitude_data.acc_x*10; 00087 IMU_CalcData.acc_y_calib=LL_1khz_attitude_data.acc_y*10; 00088 IMU_CalcData.acc_z_calib=LL_1khz_attitude_data.acc_z*10; 00089 00090 //system is initialized as soon as values differ from 0 00091 if(IMU_CalcData.acc_z_calib&&(SYSTEM_initialized<10)) SYSTEM_initialized++; 00092 00093 RO_ALL_Data.acc_x=LL_1khz_attitude_data.acc_x*10; 00094 RO_ALL_Data.acc_y=LL_1khz_attitude_data.acc_y*10; 00095 RO_ALL_Data.acc_z=LL_1khz_attitude_data.acc_z*10; 00096 00097 RO_ALL_Data.fusion_latitude=LL_1khz_attitude_data.latitude_best_estimate; 00098 RO_ALL_Data.fusion_longitude=LL_1khz_attitude_data.longitude_best_estimate; 00099 00100 } 00101 else if(current_page==1) //page 1 00102 { 00103 IMU_CalcData.height=LL_1khz_attitude_data.height; 00104 IMU_CalcData.dheight=LL_1khz_attitude_data.dheight; 00105 00106 RO_ALL_Data.fusion_height=LL_1khz_attitude_data.height; 00107 RO_ALL_Data.fusion_dheight=LL_1khz_attitude_data.dheight; 00108 00109 RO_ALL_Data.fusion_speed_x=LL_1khz_attitude_data.speed_x_best_estimate; 00110 RO_ALL_Data.fusion_speed_y=LL_1khz_attitude_data.speed_y_best_estimate; 00111 for(i=0;i<6;i++) 00112 { 00113 RO_ALL_Data.motor_rpm[i]=LL_1khz_attitude_data.motor_data[i]; 00114 } 00115 } 00116 else if(current_page==2) 00117 { 00118 IMU_CalcData.Hx=LL_1khz_attitude_data.mag_x; 00119 IMU_CalcData.Hy=LL_1khz_attitude_data.mag_y; 00120 IMU_CalcData.Hz=LL_1khz_attitude_data.mag_z; 00121 00122 RO_ALL_Data.Hx=LL_1khz_attitude_data.mag_x; 00123 RO_ALL_Data.Hy=LL_1khz_attitude_data.mag_y; 00124 RO_ALL_Data.Hz=LL_1khz_attitude_data.mag_z; 00125 00126 unsigned char slowDataUpChannelSelect=(LL_1khz_attitude_data.status2>>1)&0x7F; 00127 switch (slowDataUpChannelSelect) 00128 { 00129 case SUDC_FLIGHTTIME: 00130 00131 HL_Status.flight_time=LL_1khz_attitude_data.slowDataUpChannelDataShort; 00132 break; 00133 case SUDC_NAVSTATUS: 00134 wpCtrlNavStatus=LL_1khz_attitude_data.slowDataUpChannelDataShort; 00135 break; 00136 case SUDC_DISTTOWP: 00137 wpCtrlDistToWp=LL_1khz_attitude_data.slowDataUpChannelDataShort; 00138 break; 00139 case SUDC_WPACKTRIGGER: 00140 wpCtrlAckTrigger=LL_1khz_attitude_data.slowDataUpChannelDataShort; 00141 break; 00142 00143 } 00144 00145 } 00146 } 00147 00148 int HL2LL_write_cycle(void) //write data to low-level processor 00149 { 00150 static char pageselect=0; 00151 unsigned char i; 00152 00153 if(!data_sent_to_LL) return(0); 00154 00155 //update 1kHz data 00156 LL_1khz_control_input.system_flags=0|pageselect; 00157 //SSP_ack=0; //reset ack 00158 00159 if(gpsDataOkTrigger) LL_1khz_control_input.system_flags|=SF_GPS_NEW; 00160 00161 if(WO_SDK.ctrl_enabled) LL_1khz_control_input.system_flags|=SF_HL_CONTROL_ENABLED|SF_NEW_SDK; 00162 else LL_1khz_control_input.system_flags&=~(SF_HL_CONTROL_ENABLED|SF_NEW_SDK); 00163 00164 if(WO_SDK.disable_motor_onoff_by_stick) LL_1khz_control_input.system_flags|=SF_SDK_DISABLE_MOTORONOFF_BY_STICK; 00165 else LL_1khz_control_input.system_flags&=~SF_SDK_DISABLE_MOTORONOFF_BY_STICK; 00166 00167 if(WO_SDK.ctrl_mode==0x00) //direct individual motor control 00168 { 00169 LL_1khz_control_input.system_flags|=SF_DIRECT_MOTOR_CONTROL_INDIVIDUAL; 00170 for(i=0;i<8;i++) 00171 { 00172 LL_1khz_control_input.direct_motor_control[i]=WO_Direct_Individual_Motor_Control.motor[i]; 00173 } 00174 } 00175 else if(WO_SDK.ctrl_mode==0x01) //direct motor control with standard output mapping 00176 { 00177 LL_1khz_control_input.system_flags|=SF_DIRECT_MOTOR_CONTROL; 00178 LL_1khz_control_input.direct_motor_control[0]=WO_Direct_Motor_Control.pitch; 00179 LL_1khz_control_input.direct_motor_control[1]=WO_Direct_Motor_Control.roll; 00180 LL_1khz_control_input.direct_motor_control[2]=WO_Direct_Motor_Control.yaw; 00181 LL_1khz_control_input.direct_motor_control[3]=WO_Direct_Motor_Control.thrust; 00182 } 00183 else if (WO_SDK.ctrl_mode==0x02) //attitude control 00184 { 00185 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" 00186 LL_1khz_control_input.ctrl_flags=WO_CTRL_Input.ctrl; 00187 LL_1khz_control_input.pitch=WO_CTRL_Input.pitch; 00188 LL_1khz_control_input.roll=WO_CTRL_Input.roll; 00189 LL_1khz_control_input.yaw=WO_CTRL_Input.yaw; 00190 LL_1khz_control_input.thrust=WO_CTRL_Input.thrust; 00191 } 00192 else if (WO_SDK.ctrl_mode==0x03) //gps waypoint control 00193 { 00194 LL_1khz_control_input.system_flags|=SF_WAYPOINT_MODE; 00195 00196 //check if new command should be send 00197 00198 if (wpCtrlWpCmdUpdated) 00199 { 00200 00201 if (wpCtrlWpCmd==WP_CMD_SINGLE_WP) 00202 { 00203 if (wpCtrlWpCmdUpdated==1) 00204 { 00205 LL_1khz_control_input.ctrl_flags&=0x00FF; 00206 LL_1khz_control_input.ctrl_flags|=WP_CMD_SINGLE_WP_PART1<<8; 00207 00208 LL_1khz_control_input.pitch=wpToLL.X&0xFFFF; 00209 LL_1khz_control_input.roll=wpToLL.X>>16; 00210 LL_1khz_control_input.thrust=wpToLL.Y&0xFFFF; 00211 LL_1khz_control_input.yaw=wpToLL.Y>>16; 00212 LL_1khz_control_input.direct_motor_control[0]=wpToLL.height; 00213 LL_1khz_control_input.direct_motor_control[1]=wpToLL.height>>8; 00214 LL_1khz_control_input.direct_motor_control[2]=wpToLL.height>>16; 00215 LL_1khz_control_input.direct_motor_control[3]=wpToLL.height>>24; 00216 LL_1khz_control_input.direct_motor_control[4]=wpToLL.yaw; 00217 LL_1khz_control_input.direct_motor_control[5]=wpToLL.yaw>>8; 00218 LL_1khz_control_input.direct_motor_control[6]=wpToLL.yaw>>16; 00219 LL_1khz_control_input.direct_motor_control[7]=wpToLL.yaw>>24; 00220 00221 wpCtrlWpCmdUpdated++; 00222 }else if (wpCtrlWpCmdUpdated==2) 00223 { 00224 LL_1khz_control_input.ctrl_flags&=0x00FF; 00225 LL_1khz_control_input.ctrl_flags|=WP_CMD_SINGLE_WP_PART2<<8; 00226 00227 LL_1khz_control_input.pitch=wpToLL.time; 00228 LL_1khz_control_input.roll=0; //wpToLL.cam_angle_pitch; 00229 LL_1khz_control_input.thrust=wpToLL.pos_acc; 00230 LL_1khz_control_input.yaw=wpToLL.chksum; 00231 LL_1khz_control_input.direct_motor_control[0]=0; //wpToLL.cam_angle_roll; 00232 LL_1khz_control_input.direct_motor_control[1]=wpToLL.max_speed; 00233 LL_1khz_control_input.direct_motor_control[2]=wpToLL.properties; 00234 LL_1khz_control_input.direct_motor_control[3]=wpToLL.wp_activated; 00235 LL_1khz_control_input.direct_motor_control[4]=0; 00236 LL_1khz_control_input.direct_motor_control[5]=0; 00237 LL_1khz_control_input.direct_motor_control[6]=0; 00238 LL_1khz_control_input.direct_motor_control[7]=0; 00239 wpCtrlWpCmdUpdated=0; 00240 wpCtrlNavStatus=0; 00241 } 00242 }else 00243 { 00244 LL_1khz_control_input.ctrl_flags&=0x00FF; 00245 LL_1khz_control_input.ctrl_flags|=wpCtrlWpCmd<<8; 00246 wpCtrlWpCmdUpdated=0; 00247 } 00248 }else 00249 { 00250 LL_1khz_control_input.ctrl_flags&=0x00FF; 00251 } 00252 } 00253 else LL_1khz_control_input.system_flags&=~(SF_DIRECT_MOTOR_CONTROL|SF_DIRECT_MOTOR_CONTROL_INDIVIDUAL|SF_WAYPOINT_MODE); 00254 00255 if(pageselect==0) 00256 { 00257 //fill struct with 500Hz data 00258 LL_1khz_control_input.latitude=GPS_Data.latitude; 00259 LL_1khz_control_input.longitude=GPS_Data.longitude; 00260 LL_1khz_control_input.height=GPS_Data.height; 00261 LL_1khz_control_input.speed_x=GPS_Data.speed_x; 00262 LL_1khz_control_input.speed_y=GPS_Data.speed_y; 00263 LL_1khz_control_input.heading=GPS_Data.heading; 00264 LL_1khz_control_input.status=GPS_Data.status; 00265 00266 //write data 00267 LL_write_ctrl_data(pageselect); 00268 //set pageselect to other page for next cycle 00269 pageselect=1; 00270 } 00271 else //pageselect=1 00272 { 00273 //fill struct with 500Hz data 00274 LL_1khz_control_input.hor_accuracy=GPS_Data.horizontal_accuracy; 00275 LL_1khz_control_input.vert_accuracy=GPS_Data.vertical_accuracy; 00276 LL_1khz_control_input.speed_accuracy=GPS_Data.speed_accuracy; 00277 LL_1khz_control_input.numSV=GPS_Data.numSV; 00278 LL_1khz_control_input.battery_voltage_1=HL_Status.battery_voltage_1; 00279 LL_1khz_control_input.battery_voltage_2=HL_Status.battery_voltage_2; 00280 if (declinationAvailable==1) 00281 { 00282 declinationAvailable=2; 00283 LL_1khz_control_input.slowDataChannelSelect=SDC_DECLINATION; 00284 LL_1khz_control_input.slowDataChannelDataShort=estimatedDeclination; 00285 00286 } 00287 else if (declinationAvailable==2) 00288 { 00289 declinationAvailable=3; 00290 LL_1khz_control_input.slowDataChannelSelect=SDC_INCLINATION; 00291 LL_1khz_control_input.slowDataChannelDataShort=estimatedInclination; 00292 00293 } 00294 else 00295 { 00296 LL_1khz_control_input.slowDataChannelDataShort=0; 00297 LL_1khz_control_input.slowDataChannelSelect=0; 00298 LL_1khz_control_input.slowDataChannelDataChar=0; 00299 } 00300 00301 00302 //write data 00303 LL_write_ctrl_data(pageselect); 00304 //set pageselect to other page for next cycle 00305 pageselect=0; 00306 } 00307 return(1); 00308 } 00309 00310 void LL_write_ctrl_data(char page) 00311 { 00312 unsigned int i; 00313 unsigned char *dataptr; 00314 static volatile short spi_chksum; 00315 00316 dataptr=(unsigned char *)&LL_1khz_control_input; 00317 00318 //initialize syncbytes 00319 SPIWRData[0]='>'; 00320 SPIWRData[1]='*'; 00321 00322 spi_chksum=0xAAAA; 00323 00324 if(!page) 00325 { 00326 for(i=2;i<40;i++) 00327 { 00328 SPIWRData[i]=*dataptr++; 00329 spi_chksum+=SPIWRData[i]; 00330 } 00331 } 00332 else 00333 { 00334 for(i=2;i<22;i++) 00335 { 00336 SPIWRData[i]=*dataptr++; 00337 spi_chksum+=SPIWRData[i]; 00338 } 00339 dataptr+=18; 00340 for(i=22;i<40;i++) 00341 { 00342 SPIWRData[i]=*dataptr++; 00343 spi_chksum+=SPIWRData[i]; 00344 } 00345 } 00346 00347 SPIWRData[40]=spi_chksum; //chksum LSB 00348 SPIWRData[41]=(spi_chksum>>8); //chksum MSB 00349 00350 SPIWR_num_bytes=42; 00351 data_sent_to_LL=0; 00352 } 00353 00354 00355 inline void SSP_rx_handler_HL(unsigned char SPI_rxdata) //rx_handler @ high-level processor 00356 { 00357 static volatile unsigned char SPI_syncstate=0; 00358 static volatile unsigned char SPI_rxcount=0; 00359 static volatile unsigned char *SPI_rxptr; 00360 static volatile unsigned char incoming_page; 00361 00362 //receive handler 00363 if (SPI_syncstate==0) 00364 { 00365 if (SPI_rxdata=='>') SPI_syncstate++; else SPI_syncstate=0; 00366 } 00367 else if (SPI_syncstate==1) 00368 { 00369 if (SPI_rxdata=='*') 00370 { 00371 SPI_syncstate++; 00372 SPI_rxptr=(unsigned char *)&LL_1khz_attitude_data; 00373 SPI_rxcount=40; 00374 } 00375 else SPI_syncstate=0; 00376 } 00377 else if (SPI_syncstate==2) 00378 { 00379 if(SPI_rxcount==26) //14 bytes transmitted => select 500Hz page 00380 { 00381 incoming_page=LL_1khz_attitude_data.system_flags&0x03; //system flags were already received 00382 if(incoming_page==1) SPI_rxptr+=26; 00383 else if(incoming_page==2) SPI_rxptr+=52; 00384 else if(incoming_page>2) SPI_syncstate = 0; 00385 } 00386 SPI_rxcount--; 00387 *SPI_rxptr=SPI_rxdata; 00388 SPI_rxptr++; 00389 if (SPI_rxcount==0) 00390 { 00391 SPI_syncstate++; 00392 } 00393 } 00394 else if (SPI_syncstate==3) 00395 { 00396 if(SPI_rxdata=='<') //last byte ok => data should be valid 00397 { 00398 SSP_data_distribution_HL(); //only distribute data to other structs, if it was received correctly 00399 //ack data receiption 00400 } 00401 SPI_syncstate=0; 00402 } 00403 else SPI_syncstate=0; 00404 } 00405