LL_HL_comm.c
Go to the documentation of this file.
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 


asctec_hl_firmware
Author(s): Markus Achtelik, Michael Achtelik, Stephan Weiss, Laurent Kneip
autogenerated on Tue Dec 17 2013 11:39:27