flight/modules/UAVOHottBridge/uavohottbridge.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup UAVOHoTTBridge UAVO to HoTT Bridge Module
   6  * @{
   7  *
   8  * @file       uavohottridge.c
   9  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017.
  10  * @brief      Bridges certain UAVObjects to HoTT on USB VCP
  11  *
  12  * @see        The GNU Public License (GPL) Version 3
  13  *
  14  *****************************************************************************/
  15 /*
  16  * This program is free software; you can redistribute it and/or modify
  17  * it under the terms of the GNU General Public License as published by
  18  * the Free Software Foundation; either version 3 of the License, or
  19  * (at your option) any later version.
  20  *
  21  * This program is distributed in the hope that it will be useful, but
  22  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  23  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  24  * for more details.
  25  *
  26  * You should have received a copy of the GNU General Public License along
  27  * with this program; if not, write to the Free Software Foundation, Inc.,
  28  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  29  *
  30  * Additional note on redistribution: The copyright and license notices above
  31  * must be maintained in each individual source file that is a derivative work
  32  * of this source file; otherwise redistribution is prohibited.
  33  */
  34
  35 #include "openpilot.h"
  36 #include "hwsettings.h"
  37 #include "taskinfo.h"
  38 #include "callbackinfo.h"
  39 /*
  40 #include "insgps.h"
  41    #include "receiverstatus.h"
  42    #include "flightmodesettings.h"
  43    #include "flightbatterysettings.h"
  44    #include "flightbatterystate.h"
  45    #include "gpspositionsensor.h"
  46    #include "manualcontrolsettings.h"
  47    #include "oplinkstatus.h"
  48    #include "accessorydesired.h"
  49    #include "airspeedstate.h"
  50    #include "actuatorsettings.h"
  51    #include "systemstats.h"
  52    #include "systemalarms.h"
  53    #include "takeofflocation.h"
  54    #include "homelocation.h"
  55    #include "stabilizationdesired.h"
  56    #include "stabilizationsettings.h"
  57    #include "stabilizationbank.h"
  58    #include "stabilizationsettingsbank1.h"
  59    #include "stabilizationsettingsbank2.h"
  60    #include "stabilizationsettingsbank3.h"
  61    #include "magstate.h"
  62 #include "manualcontrolcommand.h"
  63 #include "accessorydesired.h"
  64 #include "actuatordesired.h"
  65 #include "auxpositionsensor.h"
  66 #include "pathdesired.h"
  67 #include "poilocation.h"
  68 #include "flightmodesettings.h"
  69 #include "flightstatus.h"
  70 #include "positionstate.h"
  71 #include "velocitystate.h"
  72 #include "attitudestate.h"
  73 #include "gyrostate.h"
  74  */
  75
  76 #include "hottbridgestatus.h"
  77 #include "objectpersistence.h"
  78
  79 #include "pios_sensors.h"
  80
  81 #if defined(PIOS_INCLUDE_HoTT_BRIDGE)
  82
  83 struct hott_bridge {
  84     uintptr_t     com;
  85
  86     uint32_t      lastPingTimestamp;
  87     uint8_t       myPingSequence;
  88     uint8_t       remotePingSequence;
  89     PiOSDeltatimeConfig roundtrip;
  90     double        roundTripTime;
  91     int32_t       pingTimer;
  92     int32_t       stateTimer;
  93     int32_t       rateTimer;
  94     float         rateAccumulator[3];
  95     uint8_t       rx_buffer[HoTTBRIDGEMESSAGE_BUFFERSIZE];
  96     size_t        rx_length;
  97     volatile bool scheduled[HoTTBRIDGEMESSAGE_END_ARRAY_SIZE];
  98 };
  99
 100 #if defined(PIOS_HoTT_STACK_SIZE)
 101 #define STACK_SIZE_BYTES  PIOS_HoTT_STACK_SIZE
 102 #else
 103 #define STACK_SIZE_BYTES  2048
 104 #endif
 105 #define TASK_PRIORITY     CALLBACK_TASK_AUXILIARY
 106 #define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR
 107 #define CBTASK_PRIORITY   CALLBACK_TASK_AUXILIARY
 108
 109 static bool module_enabled = false;
 110 static struct hott_bridge *hott;
 111 static int32_t uavoHoTTBridgeInitialize(void);
 112 static void uavoHoTTBridgeRxTask(void *parameters);
 113 static void uavoHoTTBridgeTxTask(void);
 114 static DelayedCallbackInfo *callbackHandle;
 115 static hottbridgemessage_handler ping_handler, ping_r_handler, pong_handler, pong_r_handler, fullstate_estimate_handler, imu_average_handler, gimbal_estimate_handler, flightcontrol_r_handler, pos_estimate_r_handler;
 116 static HoTTBridgeSettingsData settings;
 117 void AttitudeCb(__attribute__((unused)) UAVObjEvent *ev);
 118 void RateCb(__attribute__((unused)) UAVObjEvent *ev);
 119
 120 static hottbridgemessage_handler *const hottbridgemessagehandlers[HoTTBRIDGEMESSAGE_END_ARRAY_SIZE] = {
 121     ping_handler,
 122     NULL,
 123     NULL,
 124     NULL,
 125     pong_handler,
 126     fullstate_estimate_handler,
 127     imu_average_handler,
 128     gimbal_estimate_handler
 129 };
 130
 131
 132 /**
 133  * Process incoming bytes from an HoTT query thing.
 134  * @param[in] b received byte
 135  * @return true if we should continue processing bytes
 136  */
 137 static void hott_receive_byte(struct hott_bridge *m, uint8_t b)
 138 {
 139     m->rx_buffer[m->rx_length] = b;
 140     m->rx_length++;
 141     hottbridgemessage_t *message = (hottbridgemessage_t *)m->rx_buffer;
 142
 143     // very simple parser - but not a state machine, just a few checks
 144     if (m->rx_length <= offsetof(hottbridgemessage_t, length)) {
 145         // check (partial) magic number - partial is important since we need to restart at any time if garbage is received
 146         uint32_t canary = 0xff;
 147         for (uint32_t t = 1; t < m->rx_length; t++) {
 148             canary = (canary << 8) || 0xff;
 149         }
 150         if ((message->magic & canary) != (HoTTBRIDGEMAGIC & canary)) {
 151             // parse error, not beginning of message
 152             goto rxfailure;
 153         }
 154     }
 155     if (m->rx_length == offsetof(hottbridgemessage_t, timestamp)) {
 156         if (message->length > (uint32_t)(HoTTBRIDGEMESSAGE_BUFFERSIZE - offsetof(hottbridgemessage_t, data))) {
 157             // parse error, no messages are that long
 158             goto rxfailure;
 159         }
 160     }
 161     if (m->rx_length == offsetof(hottbridgemessage_t, crc32)) {
 162         if (message->type >= HoTTBRIDGEMESSAGE_END_ARRAY_SIZE) {
 163             // parse error
 164             goto rxfailure;
 165         }
 166         if (message->length != HoTTBRIDGEMESSAGE_SIZES[message->type]) {
 167             // parse error
 168             goto rxfailure;
 169         }
 170     }
 171     if (m->rx_length < offsetof(hottbridgemessage_t, data)) {
 172         // not a parse failure, just not there yet
 173         return;
 174     }
 175     if (m->rx_length == offsetof(hottbridgemessage_t, data) + HoTTBRIDGEMESSAGE_SIZES[message->type]) {
 176         // complete message received and stored in pointer "message"
 177         // empty buffer for next message
 178         m->rx_length = 0;
 179
 180         if (PIOS_CRC32_updateCRC(0xffffffff, message->data, message->length) != message->crc32) {
 181             // crc mismatch
 182             goto rxfailure;
 183         }
 184         uint32_t rxpackets;
 185         HoTTBridgeStatusRxPacketsGet(&rxpackets);
 186         rxpackets++;
 187         HoTTBridgeStatusRxPacketsSet(&rxpackets);
 188         switch (message->type) {
 189         case HoTTBRIDGEMESSAGE_PING:
 190             ping_r_handler(m, message);
 191             break;
 192         case HoTTBRIDGEMESSAGE_POS_ESTIMATE:
 193             pos_estimate_r_handler(m, message);
 194             break;
 195         case HoTTBRIDGEMESSAGE_FLIGHTCONTROL:
 196             flightcontrol_r_handler(m, message);
 197             break;
 198         case HoTTBRIDGEMESSAGE_GIMBALCONTROL:
 199             // TODO implement gimbal control somehow
 200             break;
 201         case HoTTBRIDGEMESSAGE_PONG:
 202             pong_r_handler(m, message);
 203             break;
 204         default:
 205             // do nothing at all and discard the message
 206             break;
 207         }
 208     }
 209     return;
 210
 211 rxfailure:
 212     m->rx_length = 0;
 213     uint32_t rxfail;
 214     HoTTBridgeStatusRxFailGet(&rxfail);
 215     rxfail++;
 216     HoTTBridgeStatusRxFailSet(&rxfail);
 217 }
 218
 219 static uint32_t hwsettings_rosspeed_enum_to_baud(uint8_t baud)
 220 {
 221     switch (baud) {
 222     case HWSETTINGS_HoTTSPEED_2400:
 223         return 2400;
 224
 225     case HWSETTINGS_HoTTSPEED_4800:
 226         return 4800;
 227
 228     case HWSETTINGS_HoTTSPEED_9600:
 229         return 9600;
 230
 231     case HWSETTINGS_HoTTSPEED_19200:
 232         return 19200;
 233
 234     case HWSETTINGS_HoTTSPEED_38400:
 235         return 38400;
 236
 237     case HWSETTINGS_HoTTSPEED_57600:
 238         return 57600;
 239
 240     default:
 241     case HWSETTINGS_HoTTSPEED_115200:
 242         return 115200;
 243     }
 244 }
 245
 246
 247 /**
 248  * Module start routine automatically called after initialization routine
 249  * @return 0 when was successful
 250  */
 251 static int32_t uavoHoTTBridgeStart(void)
 252 {
 253     if (!module_enabled) {
 254         // give port to telemetry if it doesn't have one
 255         // stops board getting stuck in condition where it can't be connected to gcs
 256         if (!PIOS_COM_TELEM_RF) {
 257             PIOS_COM_TELEM_RF = PIOS_COM_HoTT;
 258         }
 259
 260         return -1;
 261     }
 262
 263     PIOS_DELTATIME_Init(&ros->roundtrip, 1e-3f, 1e-6f, 10.0f, 1e-1f);
 264     ros->pingTimer  = 0;
 265     ros->stateTimer = 0;
 266     ros->rateTimer  = 0;
 267     ros->rateAccumulator[0] = 0;
 268     ros->rateAccumulator[1] = 0;
 269     ros->rateAccumulator[2] = 0;
 270     ros->rx_length = 0;
 271     ros->myPingSequence     = 0x66;
 272
 273     xTaskHandle taskHandle;
 274
 275     xTaskCreate(uavoHoTTBridgeRxTask, "uavoHoTTBridge", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
 276     PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_UAVOHoTTBRIDGE, taskHandle);
 277     PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
 278
 279     return 0;
 280 }
 281
 282 /**
 283  * Module initialization routine
 284  * @return 0 when initialization was successful
 285  */
 286 static int32_t uavoHoTTBridgeInitialize(void)
 287 {
 288     if (PIOS_COM_HoTT) {
 289         ros = pios_malloc(sizeof(*ros));
 290         if (ros != NULL) {
 291             memset(ros, 0x00, sizeof(*ros));
 292
 293             ros->com = PIOS_COM_HoTT;
 294
 295             HoTTBridgeStatusInitialize();
 296             AUXPositionSensorInitialize();
 297             HwSettingsInitialize();
 298             HwSettingsHoTTSpeedOptions rosSpeed;
 299             HwSettingsHoTTSpeedGet(&rosSpeed);
 300
 301             PIOS_COM_ChangeBaud(PIOS_COM_HoTT, 19200); // HoTT uses 19200 only
 302             // TODO: set COM port to 1-wire here, once PIOS_COM_ioctl is implemented
 303             callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&uavoHoTTBridgeTxTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_UAVOHoTTBRIDGE, STACK_SIZE_BYTES);
 304             //VelocityStateInitialize();
 305             //PositionStateInitialize();
 306             //AttitudeStateInitialize();
 307             //AttitudeStateConnectCallback(&AttitudeCb);
 308             //GyroStateInitialize();
 309             //GyroStateConnectCallback(&RateCb);
 310             //FlightStatusInitialize();
 311             //PathDesiredInitialize();
 312             //PoiLocationInitialize();
 313             //ActuatorDesiredInitialize();
 314             //FlightModeSettingsInitialize();
 315             //ManualControlCommandInitialize();
 316             //AccessoryDesiredInitialize();
 317
 318             module_enabled = true;
 319             return 0;
 320         }
 321     }
 322
 323     return -1;
 324 }
 325 MODULE_INITCALL(uavoHoTTBridgeInitialize, uavoHoTTBridgeStart);
 326
 327 /** various handlers **/
 328 static void ping_r_handler(struct ros_bridge *rb, rosbridgemessage_t *m)
 329 {
 330     rosbridgemessage_pingpong_t *data = (rosbridgemessage_pingpong_t *)&(m->data);
 331
 332     rb->remotePingSequence = data->sequence_number;
 333     rb->scheduled[HoTTBRIDGEMESSAGE_PONG] = true;
 334     PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
 335 }
 336
 337 static void ping_handler(struct ros_bridge *rb, rosbridgemessage_t *m)
 338 {
 339     rosbridgemessage_pingpong_t *data = (rosbridgemessage_pingpong_t *)&(m->data);
 340
 341     data->sequence_number = ++rb->myPingSequence;
 342     rb->roundtrip.last    = PIOS_DELAY_GetRaw();
 343 }
 344
 345 static void pong_r_handler(struct ros_bridge *rb, rosbridgemessage_t *m)
 346 {
 347     rosbridgemessage_pingpong_t *data = (rosbridgemessage_pingpong_t *)&(m->data);
 348
 349     if (data->sequence_number != rb->myPingSequence) {
 350         return;
 351     }
 352     float roundtrip = PIOS_DELTATIME_GetAverageSeconds(&(rb->roundtrip));
 353     HoTTBridgeStatusPingRoundTripTimeSet(&roundtrip);
 354 }
 355
 356 static void flightcontrol_r_handler(__attribute__((unused)) struct ros_bridge *rb, rosbridgemessage_t *m)
 357 {
 358     rosbridgemessage_flightcontrol_t *data = (rosbridgemessage_flightcontrol_t *)&(m->data);
 359     FlightStatusFlightModeOptions mode;
 360
 361     FlightStatusFlightModeGet(&mode);
 362     if (mode != FLIGHTSTATUS_FLIGHTMODE_HoTTCONTROLLED) {
 363         return;
 364     }
 365     PathDesiredData pathDesired;
 366     PathDesiredGet(&pathDesired);
 367     switch (data->mode) {
 368     case HoTTBRIDGEMESSAGE_FLIGHTCONTROL_MODE_WAYPOINT:
 369     {
 370         FlightModeSettingsPositionHoldOffsetData offset;
 371         FlightModeSettingsPositionHoldOffsetGet(&offset);
 372         pathDesired.End.North        = boundf(data->control[0], settings.GeoFenceBoxMin.North, settings.GeoFenceBoxMax.North);
 373         pathDesired.End.East         = boundf(data->control[1], settings.GeoFenceBoxMin.East, settings.GeoFenceBoxMax.East);
 374         pathDesired.End.Down         = boundf(data->control[2], settings.GeoFenceBoxMin.Down, settings.GeoFenceBoxMax.Down);
 375         pathDesired.Start.North      = pathDesired.End.North + offset.Horizontal;
 376         pathDesired.Start.East       = pathDesired.End.East;
 377         pathDesired.Start.Down       = pathDesired.End.Down;
 378         pathDesired.StartingVelocity = 0.0f;
 379         pathDesired.EndingVelocity   = 0.0f;
 380         pathDesired.Mode = PATHDESIRED_MODE_GOTOENDPOINT;
 381     }
 382     break;
 383     case HoTTBRIDGEMESSAGE_FLIGHTCONTROL_MODE_ATTITUDE:
 384     {
 385         pathDesired.ModeParameters[0] = data->control[0];
 386         pathDesired.ModeParameters[1] = data->control[1];
 387         pathDesired.ModeParameters[2] = data->control[2];
 388         pathDesired.ModeParameters[3] = data->control[3];
 389         pathDesired.Mode = PATHDESIRED_MODE_FIXEDATTITUDE;
 390     }
 391     break;
 392     }
 393     PathDesiredSet(&pathDesired);
 394     PoiLocationData poiLocation;
 395     PoiLocationGet(&poiLocation);
 396     poiLocation.North = data->poi[0];
 397     poiLocation.East  = data->poi[1];
 398     poiLocation.Down  = data->poi[2];
 399     PoiLocationSet(&poiLocation);
 400 }
 401 static void pos_estimate_r_handler(__attribute__((unused)) struct ros_bridge *rb, rosbridgemessage_t *m)
 402 {
 403     rosbridgemessage_pos_estimate_t *data = (rosbridgemessage_pos_estimate_t *)&(m->data);
 404     AUXPositionSensorData pos;
 405
 406     pos.North = data->position[0];
 407     pos.East  = data->position[1];
 408     pos.Down  = data->position[2];
 409     AUXPositionSensorSet(&pos);
 410 }
 411
 412 static void pong_handler(struct ros_bridge *rb, rosbridgemessage_t *m)
 413 {
 414     rosbridgemessage_pingpong_t *data = (rosbridgemessage_pingpong_t *)&(m->data);
 415
 416
 417     data->sequence_number = rb->remotePingSequence;
 418 }
 419
 420 static void fullstate_estimate_handler(__attribute__((unused)) struct ros_bridge *rb, rosbridgemessage_t *m)
 421 {
 422     PositionStateData pos;
 423     VelocityStateData vel;
 424     AttitudeStateData att;
 425     FlightStatusFlightModeOptions mode;
 426     FlightStatusArmedOptions armed;
 427     float thrust;
 428     AccessoryDesiredData accessory;
 429     ManualControlCommandData manualcontrol;
 430
 431     ManualControlCommandGet(&manualcontrol);
 432     FlightStatusArmedGet(&armed);
 433     FlightStatusFlightModeGet(&mode);
 434     ActuatorDesiredThrustGet(&thrust);
 435     PositionStateGet(&pos);
 436     VelocityStateGet(&vel);
 437     AttitudeStateGet(&att);
 438     rosbridgemessage_fullstate_estimate_t *data = (rosbridgemessage_fullstate_estimate_t *)&(m->data);
 439     data->quaternion[0] = att.q1;
 440     data->quaternion[1] = att.q2;
 441     data->quaternion[2] = att.q3;
 442     data->quaternion[3] = att.q4;
 443     data->position[0]   = pos.North;
 444     data->position[1]   = pos.East;
 445     data->position[2]   = pos.Down;
 446     data->velocity[0]   = vel.North;
 447     data->velocity[1]   = vel.East;
 448     data->velocity[2]   = vel.Down;
 449     data->rotation[0]   = ros->rateAccumulator[0];
 450     data->rotation[1]   = ros->rateAccumulator[1];
 451     data->rotation[2]   = ros->rateAccumulator[2];
 452     data->thrust = thrust;
 453     data->HoTTControlled = (mode == FLIGHTSTATUS_FLIGHTMODE_HoTTCONTROLLED) ? 1 : 0;
 454     data->FlightMode    = manualcontrol.FlightModeSwitchPosition;
 455     data->armed = (armed == FLIGHTSTATUS_ARMED_ARMED) ? 1 : 0;
 456     data->controls[0]   = manualcontrol.Roll;
 457     data->controls[1]   = manualcontrol.Pitch;
 458     data->controls[2]   = manualcontrol.Yaw;
 459     data->controls[3]   = manualcontrol.Thrust;
 460     data->controls[4]   = manualcontrol.Collective;
 461     data->controls[5]   = manualcontrol.Throttle;
 462     for (int t = 0; t < 4; t++) {
 463         if (AccessoryDesiredInstGet(t, &accessory) == 0) {
 464             data->accessory[t] = accessory.AccessoryVal;
 465         }
 466     }
 467
 468     int x, y;
 469     float *P[13];
 470     INSGetPAddress(P);
 471     for (x = 0; x < 10; x++) {
 472         for (y = 0; y < 10; y++) {
 473             data->matrix[x * 10 + y] = P[x][y];
 474         }
 475     }
 476     if (ros->rateTimer >= 1) {
 477         float factor = 1.0f / (float)ros->rateTimer;
 478         data->rotation[0] *= factor;
 479         data->rotation[1] *= factor;
 480         data->rotation[2] *= factor;
 481         ros->rateAccumulator[0] = 0;
 482         ros->rateAccumulator[1] = 0;
 483         ros->rateAccumulator[2] = 0;
 484         ros->rateTimer = 0;
 485     }
 486 }
 487 static void imu_average_handler(__attribute__((unused)) struct ros_bridge *rb, __attribute__((unused)) rosbridgemessage_t *m)
 488 {
 489     // TODO
 490 }
 491 static void gimbal_estimate_handler(__attribute__((unused)) struct ros_bridge *rb, __attribute__((unused)) rosbridgemessage_t *m)
 492 {
 493     // TODO
 494 }
 495
 496 /**
 497  * Main task routine
 498  * @param[in] parameters parameter given by PIOS_Callback_Create()
 499  */
 500 static void uavoHoTTBridgeTxTask(void)
 501 {
 502     uint8_t buffer[HoTTBRIDGEMESSAGE_BUFFERSIZE]; // buffer on the stack? could also be in static RAM but not reuseale by other callbacks then
 503     rosbridgemessage_t *message = (rosbridgemessage_t *)buffer;
 504
 505     for (rosbridgemessagetype_t type = HoTTBRIDGEMESSAGE_PING; type < HoTTBRIDGEMESSAGE_END_ARRAY_SIZE; type++) {
 506         if (ros->scheduled[type] && rosbridgemessagehandlers[type] != NULL) {
 507             message->magic       = HoTTBRIDGEMAGIC;
 508             message->length      = HoTTBRIDGEMESSAGE_SIZES[type];
 509             message->type        = type;
 510             message->timestamp   = PIOS_DELAY_GetuS();
 511             (*rosbridgemessagehandlers[type])(ros, message);
 512             message->crc32       = PIOS_CRC32_updateCRC(0xffffffff, message->data, message->length);
 513             int32_t ret = PIOS_COM_SendBufferNonBlocking(ros->com, buffer, offsetof(rosbridgemessage_t, data) + message->length);
 514             // int32_t ret = PIOS_COM_SendBuffer(ros->com, buffer, offsetof(rosbridgemessage_t, data) + message->length);
 515             ros->scheduled[type] = false;
 516             if (ret >= 0) {
 517                 uint32_t txpackets;
 518                 HoTTBridgeStatusTxPacketsGet(&txpackets);
 519                 txpackets++;
 520                 HoTTBridgeStatusTxPacketsSet(&txpackets);
 521             } else {
 522                 uint32_t txfail;
 523                 HoTTBridgeStatusTxFailGet(&txfail);
 524                 txfail++;
 525                 HoTTBridgeStatusTxFailSet(&txfail);
 526             }
 527             PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
 528             return;
 529         }
 530     }
 531     // nothing scheduled, do a ping in 10 secods time
 532 }
 533
 534 /**
 535  * Event Callback on Gyro updates (called with PIOS_SENSOR_RATE - roughly 500 Hz - from State Estimation)
 536  */
 537 void RateCb(__attribute__((unused)) UAVObjEvent *ev)
 538 {
 539     GyroStateData gyr;
 540
 541     GyroStateGet(&gyr);
 542     ros->rateAccumulator[0] += gyr.x;
 543     ros->rateAccumulator[1] += gyr.y;
 544     ros->rateAccumulator[2] += gyr.z;
 545     ros->rateTimer++;
 546 }
 547
 548 /**
 549  * Event Callback on Attitude updates (called with PIOS_SENSOR_RATE - roughly 500 Hz - from State Estimation)
 550  */
 551 void AttitudeCb(__attribute__((unused)) UAVObjEvent *ev)
 552 {
 553     bool dispatch = false;
 554
 555     if (++ros->pingTimer >= settings.UpdateRate.Ping && settings.UpdateRate.Ping > 0) {
 556         ros->pingTimer = 0;
 557         dispatch = true;
 558         ros->scheduled[HoTTBRIDGEMESSAGE_PING] = true;
 559     }
 560     if (++ros->stateTimer >= settings.UpdateRate.State && settings.UpdateRate.State > 0) {
 561         ros->stateTimer = 0;
 562         dispatch = true;
 563         ros->scheduled[HoTTBRIDGEMESSAGE_FULLSTATE_ESTIMATE] = true;
 564     }
 565     if (dispatch && callbackHandle) {
 566         PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
 567     }
 568 }
 569
 570 /**
 571  * Main task routine
 572  * @param[in] parameters parameter given by PIOS_Thread_Create()
 573  */
 574 static void uavoHoTTBridgeRxTask(__attribute__((unused)) void *parameters)
 575 {
 576     while (1) {
 577         uint8_t b = 0;
 578         uint16_t count = PIOS_COM_ReceiveBuffer(ros->com, &b, 1, ~0);
 579         if (count) {
 580             ros_receive_byte(ros, b);
 581         }
 582     }
 583 }
 584
 585 #endif // PIOS_INCLUDE_HoTT_BRIDGE
 586 /**
 587  * @}
 588  * @}
 589  */