sdk.c
Go to the documentation of this file.
00001 /*
00002 
00003  Copyright (c) 2011, Markus Achtelik, ASL, ETH Zurich, Switzerland
00004  You can contact the author at <markus dot achtelik at mavt dot ethz dot ch>
00005 
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  * Redistributions of source code must retain the above copyright
00011  notice, this list of conditions and the following disclaimer.
00012  * Redistributions in binary form must reproduce the above copyright
00013  notice, this list of conditions and the following disclaimer in the
00014  documentation and/or other materials provided with the distribution.
00015  * Neither the name of ETHZ-ASL nor the
00016  names of its contributors may be used to endorse or promote products
00017  derived from this software without specific prior written permission.
00018 
00019  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00020  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022  DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY
00023  DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00024  (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026  ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00028  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 
00030  */
00031 
00032 #include "main.h"
00033 #include "system.h"
00034 #include "sdk.h"
00035 #include "LL_HL_comm.h"
00036 #include "uart.h"
00037 #include "time.h"
00038 #include "irq.h"
00039 #include "LPC214x.h"
00040 #include "hardware.h"
00041 
00042 #include <ekf/autogen_ekf_propagation.h>
00043 #include <ekf/autogen_ekf_propagation_initialize.h>
00044 
00045 struct WO_SDK_STRUCT WO_SDK;
00046 struct WO_CTRL_INPUT WO_CTRL_Input;
00047 struct RO_RC_DATA RO_RC_Data;
00048 struct RO_ALL_DATA RO_ALL_Data;
00049 struct WO_DIRECT_MOTOR_CONTROL WO_Direct_Motor_Control;
00050 struct WO_DIRECT_INDIVIDUAL_MOTOR_CONTROL WO_Direct_Individual_Motor_Control;
00051 
00052 #include "gpsmath.h"
00053 
00054 #include <ert_main.h>
00055 #include "dekf.h"
00056 
00057 // time synchronization
00058 volatile int64_t timestamp = 0;
00059 int64_t timeOffset = 0;
00060 unsigned short time_step = 2000;
00061 int64_t time_correction = 0;
00062 
00063 short cmdLLValid = 0;
00064 unsigned char cmdLLNew = 0;
00065 
00066 short extPositionValid = 0;
00067 
00068 unsigned int cpuLoad = 0;
00069 unsigned int sdkCycleStartTime = 0;
00070 
00071 HLI_IMU imuData;
00072 HLI_RCDATA rcData;
00073 HLI_GPS gpsData;
00074 HLI_SSDK_STATUS ssdk_status;
00075 HLI_MAG mag_data;
00076 
00077 // declared external in sdk.h, so that the ssdk can access it
00078 HLI_EXT_POSITION extPosition;
00079 HLI_STATUS statusData;
00080 
00081 short motor_state = -1;
00082 short motor_state_count = 0;
00083 unsigned int sdkLoops = 0;
00084 
00085 unsigned int ssdk_reset_state = 0;
00086 
00087 // dekf variables
00088 DekfContext dekf;
00089 
00090 // set up data structures for receiving data ##################################
00091 
00092 // create structures containing data
00093 HLI_CMD_LL cmdLL;
00094 PacketInfo *packetCmdLL;
00095 
00096 // do that for every other package ...
00097 HLI_MOTORS motors;
00098 PacketInfo *packetMotors;
00099 
00100 HLI_EXT_POSITION ext_position_update;
00101 PacketInfo *packetExtPosition;
00102 
00103 // extPositionCmd is declared external in sdk.h, so that the ssdk can access it
00104 HLI_CMD_HL extPositionCmd;
00105 PacketInfo *packetCmdHL;
00106 
00107 HLI_TIMESYNC timeSync;
00108 PacketInfo *packetTimeSync;
00109 
00110 PacketInfo *packetSSDKParams;
00111 
00112 // control enable packet
00113 HLI_CMD_LL controlEnable;
00114 PacketInfo *packetControlEnable;
00115 
00116 // EKF state and packet info are defined in dekf.h
00117 
00118 HLI_BAUDRATE baudrate;
00119 PacketInfo *packetBaudrate;
00120 
00121 // subscription packet
00122 HLI_SUBSCRIPTION subscription;
00123 PacketInfo *packetSubscription;
00124 
00125 // config packet, declared external in sdk.h
00126 HLI_CONFIG hli_config;
00127 PacketInfo *packetConfig;
00128 
00129 // ######################################################################################
00130 
00131 void sdkInit(void)
00132 {
00133   //    UART_setPacketInfo(packetInfo);
00134   statusData.have_SSDK_parameters = 0;
00135   ssdk_status.have_parameters = 0;
00136   baudrate.state = 0;
00137 
00138   extPositionValid = 0;
00139 
00140   hli_config.mode_position_control = HLI_MODE_POSCTRL_OFF;
00141   hli_config.mode_state_estimation = HLI_MODE_STATE_ESTIMATION_OFF;
00142   hli_config.position_control_axis_enable = 0;
00143   hli_config.battery_warning_voltage = BATTERY_WARNING_VOLTAGE;
00144 
00145   // set default packet rates
00146   subscription.imu = HLI_DEFAULT_PERIOD_IMU;
00147   subscription.rcdata = HLI_DEFAULT_PERIOD_RCDATA;
00148   subscription.gps = HLI_DEFAULT_PERIOD_GPS;
00149   subscription.ssdk_debug = HLI_DEFAULT_PERIOD_SSDK_DEBUG;
00150   subscription.ekf_state = HLI_DEFAULT_PERIOD_EKF_STATE;
00151 
00152   // register packets to receive
00153   packetExtPosition = registerPacket(HLI_PACKET_ID_POSITION_UPDATE, &ext_position_update);
00154   packetCmdHL = registerPacket(HLI_PACKET_ID_CONTROL_HL, &extPositionCmd);
00155   packetMotors = registerPacket(HLI_PACKET_ID_MOTORS, &motors);
00156   packetCmdLL = registerPacket(HLI_PACKET_ID_CONTROL_LL, &cmdLL);
00157   packetTimeSync = registerPacket(HLI_PACKET_ID_TIMESYNC, &timeSync);
00158   packetSSDKParams = registerPacket(HLI_PACKET_ID_SSDK_PARAMETERS, &ssdk_params); // defined in ert_main.h
00159   packetControlEnable = registerPacket(HLI_PACKET_ID_CONTROL_ENABLE, &controlEnable);
00160   packetBaudrate = registerPacket(HLI_PACKET_ID_BAUDRATE, &baudrate);
00161   packetSubscription = registerPacket(HLI_PACKET_ID_SUBSCRIPTION, &subscription);
00162   packetConfig = registerPacket(HLI_PACKET_ID_CONFIG, &hli_config);
00163 
00164   UART0_rxFlush();
00165   UART0_txFlush();
00166 
00167   // init ssdk
00168   onboard_matlab_initialize();
00169 
00170   // init dekf, also packet subscription takes place here
00171   DEKF_init(&dekf, &extPosition);
00172 
00173   extPosition.bitfield = 0;
00174   extPosition.count = 0;
00175   extPosition.heading = 0;
00176   extPosition.x = 0;
00177   extPosition.y = 0;
00178   extPosition.z = 0;
00179   extPosition.vX = 0;
00180   extPosition.vY = 0;
00181   extPosition.vZ = 0;
00182   extPosition.qualX = 0;
00183   extPosition.qualY = 0;
00184   extPosition.qualZ = 0;
00185   extPosition.qualVx = 0;
00186   extPosition.qualVy = 0;
00187   extPosition.qualVz = 0;
00188 
00189   extPositionCmd.heading = 0;
00190   extPositionCmd.bitfield = 0;
00191   extPositionCmd.x = 0;
00192   extPositionCmd.y = 0;
00193   extPositionCmd.z = 0;
00194   extPositionCmd.vZ = 2000;
00195   extPositionCmd.vY = 2000;
00196   extPositionCmd.vZ = 2000;
00197   extPositionCmd.vYaw = 45000;
00198 
00199   startAutoBaud();
00200 }
00201 
00218 void SDK_mainloop(void)
00219 {
00220   sdkCycleStartTime = T1TC;
00221 
00222   WO_SDK.ctrl_mode = 0x02; // attitude and throttle control
00223   WO_SDK.disable_motor_onoff_by_stick = 0;
00224 
00225   sdkLoops++;
00226 
00227   parseRxFifo();
00228 
00229   // check for new LL command packet
00230   if (packetCmdLL->updated)
00231   {
00232     cmdLLNew = 1;
00233     packetCmdLL->updated = 0;
00234   }
00235 
00236   // check if LL commands arrive at max every CMD_MAX_PERIOD ms
00237   if ((sdkLoops % CMD_MAX_PERIOD) == 0)
00238   {
00239 
00240     if (cmdLLNew == 1)
00241     {
00242       cmdLLNew = 0;
00243       cmdLLValid++;
00244     }
00245     else
00246     {
00247       cmdLLValid--;
00248     }
00249 
00250     if (cmdLLValid < -2)
00251       cmdLLValid = -2; // min 3 packets required
00252     else if (cmdLLValid > 3)
00253       cmdLLValid = 3; // fall back after 3 missed packets
00254   }
00255 
00256   // check for motor start/stop packet
00257   if (packetMotors->updated)
00258   {
00259     motor_state = motors.motors;
00260     motor_state_count = 0;
00261     packetMotors->updated = 0;
00262   }
00263 
00264   // check for new HL command packet
00265   if (packetCmdHL->updated)
00266   {
00267     packetCmdHL->updated = 0;
00268     // SSDK operates in NED, need to convert from ENU
00269     extPositionCmd.heading = -extPositionCmd.heading + 360000;
00270     extPositionCmd.y = -extPositionCmd.y;
00271     extPositionCmd.z = -extPositionCmd.z;
00272     extPositionCmd.vY = -extPositionCmd.vY;
00273     extPositionCmd.vZ = -extPositionCmd.vZ;
00274     extPositionCmd.vYaw = -extPositionCmd.vYaw;
00275   }
00276 
00277   // handle parameter packet
00278   if (packetSSDKParams->updated == 1)
00279   {
00280     packetSSDKParams->updated = 0;
00281     statusData.have_SSDK_parameters = 1;
00282     ssdk_status.have_parameters = 1;
00283   }
00284 
00285   // decide which position/state input we take for position control
00286   // SSDK operates in NED --> convert from ENU
00287   switch(hli_config.mode_state_estimation){
00288     case HLI_MODE_STATE_ESTIMATION_HL_SSDK:
00289       extPositionValid = 1;
00290       extPosition.bitfield = 0;
00291       extPosition.count = ext_position_update.count;
00292       extPosition.heading = -ext_position_update.heading + 360000;
00293       extPosition.x = ext_position_update.x;
00294       extPosition.y = -ext_position_update.y;
00295       extPosition.z = -ext_position_update.z;
00296       extPosition.vX = ext_position_update.vX;
00297       extPosition.vY = -ext_position_update.vY;
00298       extPosition.vZ = -ext_position_update.vZ;
00299       extPosition.qualX = ext_position_update.qualX;
00300       extPosition.qualY = ext_position_update.qualY;
00301       extPosition.qualZ = ext_position_update.qualZ;
00302       extPosition.qualVx = ext_position_update.qualVx;
00303       extPosition.qualVy = ext_position_update.qualVy;
00304       extPosition.qualVz = ext_position_update.qualVz;
00305       break;
00306     case HLI_MODE_STATE_ESTIMATION_EXT:
00307       extPositionValid = 1;
00308       extPosition.bitfield = EXT_POSITION_BYPASS_FILTER;
00309       extPosition.count = ext_position_update.count;
00310       extPosition.heading = -ext_position_update.heading + 360000;
00311       extPosition.x = ext_position_update.x;
00312       extPosition.y = -ext_position_update.y;
00313       extPosition.z = -ext_position_update.z;
00314       extPosition.vX = ext_position_update.vX;
00315       extPosition.vY = -ext_position_update.vY;
00316       extPosition.vZ = -ext_position_update.vZ;
00317       extPosition.qualX = ext_position_update.qualX;
00318       extPosition.qualY = ext_position_update.qualY;
00319       extPosition.qualZ = ext_position_update.qualZ;
00320       extPosition.qualVx = ext_position_update.qualVx;
00321       extPosition.qualVy = ext_position_update.qualVy;
00322       extPosition.qualVz = ext_position_update.qualVz;
00323       break;
00324     case HLI_MODE_STATE_ESTIMATION_HL_EKF:
00325       DEKF_step(&dekf, timestamp);
00326       if(DEKF_getInitializeEvent(&dekf) == 1)
00327         ssdk_reset_state = 1;
00328 
00329       extPositionValid = 1;
00330       break;
00331     default:
00332       extPositionValid = 0;
00333   }
00334 
00335   // dekf initialize state machine
00336   // sets the acc/height/gps switch to 0 for 10 loops so that refmodel gets reset to the new state
00337   if (ssdk_reset_state >= 1 && ssdk_reset_state < 10)
00338   {
00339     RO_RC_Data.channel[0] = 2048;
00340     RO_RC_Data.channel[1] = 2048;
00341     RO_RC_Data.channel[2] = 2048;
00342     RO_RC_Data.channel[3] = 2048;
00343     RO_RC_Data.channel[5] = 0;
00344     ssdk_reset_state++;
00345   }
00346   else
00347   {
00348     ssdk_reset_state = 0;
00349   }
00350 
00351   // execute ssdk - only executed if ssdk parameters are available
00352   // reads position reference from extPosition
00353   // reads position/velocity command from extPositionCmd
00354   // finally writes to WO_CTRL_Input. therefore, make sure to overwrite it after this call if you don't want to have its output
00355   rt_OneStep();
00356 
00357   // --- write commands to LL ------------------------------------------------
00358 
00359   short motorsRunning = LL_1khz_attitude_data.status2 & 0x1;
00360 
00361   if (motor_state == -1 || motor_state == 2)
00362   { // motors are either stopped or running --> normal operation
00363 
00364     // commands are always written to LL by the Matlab controller, decide if we need to overwrite them
00365     if (extPositionValid > 0 && statusData.have_SSDK_parameters == 1 && hli_config.mode_position_control == HLI_MODE_POSCTRL_HL)
00366     {
00367       WO_CTRL_Input.ctrl = hli_config.position_control_axis_enable;
00368       WO_SDK.ctrl_enabled = 1;
00369       // limit yaw rate:
00370       if(WO_CTRL_Input.yaw > 1000)
00371         WO_CTRL_Input.yaw = 1000;
00372       else if(WO_CTRL_Input.yaw < -1000)
00373         WO_CTRL_Input.yaw = -1000;
00374     }
00375 
00376     else if (cmdLLValid > 0 && (hli_config.mode_position_control == HLI_MODE_POSCTRL_LL || hli_config.mode_position_control == HLI_MODE_POSCTRL_OFF))
00377     {
00378       writeCommand(cmdLL.x, -cmdLL.y, -cmdLL.yaw, cmdLL.z, hli_config.position_control_axis_enable, 1);
00379     }
00380     else
00381     {
00382       writeCommand(0, 0, 0, 0, 0, 0);
00383     }
00384   }
00385   // start / stop motors, allow commands max for 1.5 s
00386   else if (motor_state == 1)
00387   {
00388     if (motor_state_count < 1500)
00389     {
00390       if (!motorsRunning)
00391         writeCommand(0, 0, 2047, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1); // start/stop sequence, set ctrl to acc so that motors will be safely started
00392       else if (motorsRunning)
00393       {
00394         writeCommand(0, 0, 0, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1);
00395         motor_state = 2;
00396       }
00397     }
00398     else
00399     {
00400       writeCommand(0, 0, 0, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1);
00401       motor_state = -1;
00402     }
00403     motor_state_count++;
00404   }
00405   else if (motor_state == 0)
00406   {
00407     if (motor_state_count < 1500)
00408     {
00409       if (motorsRunning)
00410         writeCommand(0, 0, 2047, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1); // start/stop sequence, set ctrl to acc so that motors will be safely shut down
00411       else if (!motorsRunning)
00412       {
00413         writeCommand(0, 0, 0, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1);
00414         motor_state = -1;
00415       }
00416     }
00417     else
00418     {
00419       writeCommand(0, 0, 0, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1);
00420       motor_state = -1;
00421     }
00422     motor_state_count++;
00423   }
00424   else
00425   {
00426     // undefined state, disable everything
00427     writeCommand(0, 0, 0, 0, 0, 0);
00428   }
00429 
00430   // TODO: thrust limit in case something really goes wrong, may be removed
00431   if (WO_CTRL_Input.thrust > 4095)
00432     WO_CTRL_Input.thrust = 4095;
00433 
00434   // ------------------------------------------------------------------------
00435 
00436 
00437   // --- send data to UART 0 ------------------------------------------------
00438   if (checkTxPeriod(subscription.imu))
00439   {
00440     sendImuData();
00441   }
00442 
00443   if (checkTxPeriod(subscription.rcdata))
00444   {
00445     sendRcData();
00446   }
00447 
00448   if (checkTxPeriod(subscription.gps))
00449   {
00450     sendGpsData();
00451   }
00452 
00453   if ((sdkLoops + 20) % 500 == 0)
00454   {
00455     sendStatus();
00456 //    writePacket2Ringbuffer(HLI_PACKET_ID_SSDK_STATUS, (unsigned char*)&ssdk_status, sizeof(ssdk_status));
00457   }
00458 
00459   if (checkTxPeriod(subscription.ssdk_debug))
00460   {
00461     ssdk_debug.timestamp = timestamp;
00462     writePacket2Ringbuffer(HLI_PACKET_ID_SSDK_DEBUG, (unsigned char*)&ssdk_debug, sizeof(ssdk_debug));
00463   }
00464 
00465   if (checkTxPeriod(subscription.ekf_state))
00466   {
00467 //    sendEkfState();
00468     DEKF_sendState(&dekf, timestamp);
00469   }
00470 
00471   if (checkTxPeriod(subscription.mag))
00472   {
00473     sendMagData();
00474   }
00475 
00476 //
00477 
00478   UART_send_ringbuffer();
00479 
00480   synchronizeTime();
00481 
00482   if (packetBaudrate->updated)
00483   {
00484     packetBaudrate->updated = 0;
00485     while (!UART0_txEmpty())
00486       ;
00487   }
00488 
00489   // ------------------------------------------------------------------------
00490 
00491   unsigned int dt;
00492   if (T1TC < sdkCycleStartTime)
00493     dt = (processorClockFrequency() - sdkCycleStartTime) + T1TC;
00494   else
00495     dt = T1TC - sdkCycleStartTime;
00496 
00497   cpuLoad = ControllerCyclesPerSecond * ((dt * 1e2) / processorClockFrequency()); // cpu load in %
00498 
00499   watchdog();
00500 }
00501 
00502 inline void writeCommand(short pitch, short roll, short yaw, short thrust, short ctrl, short enable)
00503 {
00504   WO_CTRL_Input.pitch = pitch;
00505   WO_CTRL_Input.roll = roll;
00506   WO_CTRL_Input.thrust = thrust;
00507   WO_CTRL_Input.yaw = yaw;
00508   WO_CTRL_Input.ctrl = ctrl;
00509 
00510   WO_SDK.ctrl_enabled = enable;
00511 }
00512 
00513 inline void sendImuData(void)
00514 {
00515   imuData.timestamp = timestamp;
00516 
00517   // TODO: smoothing of data to make Nyquist happy ;-)
00518   // acceleration, angular velocities, attitude, height, dheight following the ENU convention (x front, y left, z up)
00519   // LL firmware 2012 is NED now
00520   imuData.acc_x = LL_1khz_attitude_data.acc_x;
00521   imuData.acc_y = -LL_1khz_attitude_data.acc_y;
00522   imuData.acc_z = -LL_1khz_attitude_data.acc_z;
00523   imuData.ang_vel_roll = LL_1khz_attitude_data.angvel_roll;
00524   imuData.ang_vel_pitch = -LL_1khz_attitude_data.angvel_pitch;
00525   imuData.ang_vel_yaw = -LL_1khz_attitude_data.angvel_yaw;
00526   imuData.ang_roll = LL_1khz_attitude_data.angle_roll;
00527   imuData.ang_pitch = -LL_1khz_attitude_data.angle_pitch;
00528   imuData.ang_yaw = 36000 - LL_1khz_attitude_data.angle_yaw;
00529   imuData.differential_height = LL_1khz_attitude_data.dheight;
00530   imuData.height = LL_1khz_attitude_data.height;
00531 
00532   writePacket2Ringbuffer(HLI_PACKET_ID_IMU, (unsigned char*)&imuData, sizeof(imuData));
00533 }
00534 
00535 inline void sendGpsData(void)
00536 {
00537 
00538   gpsData.timestamp = timestamp;
00539   //    gpsData.latitude = LL_1khz_attitude_data.latitude_best_estimate;
00540   //    gpsData.longitude = LL_1khz_attitude_data.longitude_best_estimate;
00541   gpsData.latitude = GPS_Data.latitude;
00542   gpsData.longitude = GPS_Data.longitude;
00543   gpsData.heading = LL_1khz_attitude_data.angle_yaw;
00544   gpsData.height = GPS_Data.height;
00545   gpsData.pressure_height = LL_1khz_attitude_data.height;
00546   gpsData.speedX = GPS_Data.speed_x;
00547   gpsData.speedY = GPS_Data.speed_y;
00548   gpsData.horizontalAccuracy = GPS_Data.horizontal_accuracy;
00549   gpsData.verticalAccuracy = GPS_Data.horizontal_accuracy;
00550   gpsData.speedAccuracy = GPS_Data.speed_accuracy;
00551   gpsData.numSatellites = GPS_Data.numSV;
00552   gpsData.status = GPS_Data.status;
00553 
00554   writePacket2Ringbuffer(HLI_PACKET_ID_GPS, (unsigned char*)&gpsData, sizeof(gpsData));
00555 }
00556 
00557 inline void sendStatus(void)
00558 {
00559 
00560   statusData.timestamp = timestamp;
00561   statusData.battery_voltage = LL_1khz_attitude_data.battery_voltage1;
00562   statusData.cpu_load = cpuLoad;
00563   statusData.flight_mode = LL_1khz_attitude_data.flightMode;
00564   statusData.flight_time = HL_Status.flight_time;
00565 
00566   if (motor_state == 0 || motor_state == 1)
00567     statusData.motors = motor_state;
00568   else if (LL_1khz_attitude_data.status2 & 0x1)
00569     statusData.motors = 2;
00570   else if (!(LL_1khz_attitude_data.status2 & 0x1))
00571     statusData.motors = -1;
00572 
00573 //  statusData.debug1 = uart0_min_rx_buffer;
00574 //  statusData.debug2 = uart0_min_tx_buffer;
00575 
00576   statusData.state_estimation = hli_config.mode_state_estimation;
00577   statusData.position_control = hli_config.mode_position_control;
00578 
00579   statusData.rx_packets = UART_rxPacketCount;
00580   statusData.rx_packets_good = UART_rxGoodPacketCount;
00581 
00582   writePacket2Ringbuffer(HLI_PACKET_ID_STATUS, (unsigned char*)&statusData, sizeof(statusData));
00583 }
00584 
00585 inline void sendRcData(void)
00586 {
00587   int i = 0;
00588 
00589   rcData.timestamp = timestamp;
00590   for (i = 0; i < sizeof(rcData.channel) / sizeof(unsigned short); i++)
00591     rcData.channel[i] = RO_RC_Data.channel[i];
00592 
00593   writePacket2Ringbuffer(HLI_PACKET_ID_RC, (unsigned char*)&rcData, sizeof(rcData));
00594 }
00595 
00596 inline void sendMagData(void)
00597 {
00598   mag_data.timestamp = timestamp;
00599   mag_data.x = LL_1khz_attitude_data.mag_x;
00600   mag_data.y = LL_1khz_attitude_data.mag_y;
00601   mag_data.z = LL_1khz_attitude_data.mag_z;
00602 
00603   writePacket2Ringbuffer(HLI_PACKET_ID_MAG, (unsigned char*)&mag_data, sizeof(mag_data));
00604 }
00605 
00606 inline void synchronizeTime()
00607 {
00608 
00609   // check for timesync packet
00610   if (packetTimeSync->updated)
00611   {
00612     timeOffset = (900 * timeOffset + 100 * (timeSync.ts1 * 2 - timeSync.tc1 - timestamp) / 2) / 1000;
00613     statusData.timesync_offset = timeOffset;
00614 
00615     if (timeOffset > 1e5 || timeOffset < -1e5)
00616     {
00617       timestamp = timeSync.ts1;
00618       timeOffset = 0;
00619     }
00620     else if (timeOffset > 2000)
00621       timeOffset = 2000;
00622     else if (timeOffset < -2000)
00623       timeOffset = -2000;
00624 
00625     if (timeOffset > 0)
00626     {
00627       time_step = 2000 / timeOffset;
00628       time_correction = 2;
00629     }
00630     else if (timeOffset < 0)
00631     {
00632       time_step = -2000 / timeOffset;
00633       time_correction = -2;
00634     }
00635     else
00636     {
00637       time_step = 2000;
00638       time_correction = 0;
00639     }
00640 
00641     packetTimeSync->updated = 0;
00642   }
00643 
00644   // correct timestamp every time_step sdkloops by time_correction us
00645   if (sdkLoops % time_step == 0)
00646   {
00647     timestamp += time_correction;
00648   }
00649 
00650   if (sdkLoops % 2000 == 0)
00651   {
00652     timeSync.tc1 = timestamp;
00653     timeSync.ts1 = 0;
00654     writePacket2Ringbuffer(HLI_PACKET_ID_TIMESYNC, (unsigned char*)&timeSync, sizeof(timeSync));
00655     UART_send_ringbuffer();
00656   }
00657 }
00658 
00659 inline void watchdog(void)
00660 {
00661 
00662   static uint32_t lastTxPackets = 0;
00663 
00664   // check if a valid packet arrived in the HLI_COMMUNICATION_TIMEOUT s
00665   if ((sdkLoops % (ControllerCyclesPerSecond * HLI_COMMUNICATION_TIMEOUT)) == 0)
00666   {
00667     if (UART_rxGoodPacketCount == lastTxPackets)
00668     {
00669       startAutoBaud();
00670     }
00671     lastTxPackets = UART_rxGoodPacketCount;
00672   }
00673 }
00674 
00675 inline int checkTxPeriod(uint16_t period)
00676 {
00677   if (period == 0)
00678     return 0;
00679   else
00680     return sdkLoops % period == 0;
00681 }


asctec_hl_firmware
Author(s): Markus Achtelik, Michael Achtelik, Stephan Weiss, Laurent Kneip
autogenerated on Tue Jan 7 2014 11:05:19