From 2977ec1382d16b1345231c4751246fdd0636ad2f Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 22 Mar 2017 15:52:59 +0100 Subject: [PATCH 1/7] LP-500 HoTT bridge module initial create --- .../modules/UAVOHottBridge/uavohottbridge.c | 589 ++++++++++++++++++ 1 file changed, 589 insertions(+) create mode 100644 flight/modules/UAVOHottBridge/uavohottbridge.c diff --git a/flight/modules/UAVOHottBridge/uavohottbridge.c b/flight/modules/UAVOHottBridge/uavohottbridge.c new file mode 100644 index 000000000..fdc5b0da4 --- /dev/null +++ b/flight/modules/UAVOHottBridge/uavohottbridge.c @@ -0,0 +1,589 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup UAVOHoTTBridge UAVO to HoTT Bridge Module + * @{ + * + * @file uavohottridge.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * @brief Bridges certain UAVObjects to HoTT on USB VCP + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Additional note on redistribution: The copyright and license notices above + * must be maintained in each individual source file that is a derivative work + * of this source file; otherwise redistribution is prohibited. + */ + +#include "openpilot.h" +#include "hwsettings.h" +#include "taskinfo.h" +#include "callbackinfo.h" +/* +#include "insgps.h" + #include "receiverstatus.h" + #include "flightmodesettings.h" + #include "flightbatterysettings.h" + #include "flightbatterystate.h" + #include "gpspositionsensor.h" + #include "manualcontrolsettings.h" + #include "oplinkstatus.h" + #include "accessorydesired.h" + #include "airspeedstate.h" + #include "actuatorsettings.h" + #include "systemstats.h" + #include "systemalarms.h" + #include "takeofflocation.h" + #include "homelocation.h" + #include "stabilizationdesired.h" + #include "stabilizationsettings.h" + #include "stabilizationbank.h" + #include "stabilizationsettingsbank1.h" + #include "stabilizationsettingsbank2.h" + #include "stabilizationsettingsbank3.h" + #include "magstate.h" +#include "manualcontrolcommand.h" +#include "accessorydesired.h" +#include "actuatordesired.h" +#include "auxpositionsensor.h" +#include "pathdesired.h" +#include "poilocation.h" +#include "flightmodesettings.h" +#include "flightstatus.h" +#include "positionstate.h" +#include "velocitystate.h" +#include "attitudestate.h" +#include "gyrostate.h" + */ + +#include "hottbridgestatus.h" +#include "objectpersistence.h" + +#include "pios_sensors.h" + +#if defined(PIOS_INCLUDE_HoTT_BRIDGE) + +struct hott_bridge { + uintptr_t com; + + uint32_t lastPingTimestamp; + uint8_t myPingSequence; + uint8_t remotePingSequence; + PiOSDeltatimeConfig roundtrip; + double roundTripTime; + int32_t pingTimer; + int32_t stateTimer; + int32_t rateTimer; + float rateAccumulator[3]; + uint8_t rx_buffer[HoTTBRIDGEMESSAGE_BUFFERSIZE]; + size_t rx_length; + volatile bool scheduled[HoTTBRIDGEMESSAGE_END_ARRAY_SIZE]; +}; + +#if defined(PIOS_HoTT_STACK_SIZE) +#define STACK_SIZE_BYTES PIOS_HoTT_STACK_SIZE +#else +#define STACK_SIZE_BYTES 2048 +#endif +#define TASK_PRIORITY CALLBACK_TASK_AUXILIARY +#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR +#define CBTASK_PRIORITY CALLBACK_TASK_AUXILIARY + +static bool module_enabled = false; +static struct hott_bridge *hott; +static int32_t uavoHoTTBridgeInitialize(void); +static void uavoHoTTBridgeRxTask(void *parameters); +static void uavoHoTTBridgeTxTask(void); +static DelayedCallbackInfo *callbackHandle; +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; +static HoTTBridgeSettingsData settings; +void AttitudeCb(__attribute__((unused)) UAVObjEvent *ev); +void RateCb(__attribute__((unused)) UAVObjEvent *ev); + +static hottbridgemessage_handler *const hottbridgemessagehandlers[HoTTBRIDGEMESSAGE_END_ARRAY_SIZE] = { + ping_handler, + NULL, + NULL, + NULL, + pong_handler, + fullstate_estimate_handler, + imu_average_handler, + gimbal_estimate_handler +}; + + +/** + * Process incoming bytes from an HoTT query thing. + * @param[in] b received byte + * @return true if we should continue processing bytes + */ +static void hott_receive_byte(struct hott_bridge *m, uint8_t b) +{ + m->rx_buffer[m->rx_length] = b; + m->rx_length++; + hottbridgemessage_t *message = (hottbridgemessage_t *)m->rx_buffer; + + // very simple parser - but not a state machine, just a few checks + if (m->rx_length <= offsetof(hottbridgemessage_t, length)) { + // check (partial) magic number - partial is important since we need to restart at any time if garbage is received + uint32_t canary = 0xff; + for (uint32_t t = 1; t < m->rx_length; t++) { + canary = (canary << 8) || 0xff; + } + if ((message->magic & canary) != (HoTTBRIDGEMAGIC & canary)) { + // parse error, not beginning of message + goto rxfailure; + } + } + if (m->rx_length == offsetof(hottbridgemessage_t, timestamp)) { + if (message->length > (uint32_t)(HoTTBRIDGEMESSAGE_BUFFERSIZE - offsetof(hottbridgemessage_t, data))) { + // parse error, no messages are that long + goto rxfailure; + } + } + if (m->rx_length == offsetof(hottbridgemessage_t, crc32)) { + if (message->type >= HoTTBRIDGEMESSAGE_END_ARRAY_SIZE) { + // parse error + goto rxfailure; + } + if (message->length != HoTTBRIDGEMESSAGE_SIZES[message->type]) { + // parse error + goto rxfailure; + } + } + if (m->rx_length < offsetof(hottbridgemessage_t, data)) { + // not a parse failure, just not there yet + return; + } + if (m->rx_length == offsetof(hottbridgemessage_t, data) + HoTTBRIDGEMESSAGE_SIZES[message->type]) { + // complete message received and stored in pointer "message" + // empty buffer for next message + m->rx_length = 0; + + if (PIOS_CRC32_updateCRC(0xffffffff, message->data, message->length) != message->crc32) { + // crc mismatch + goto rxfailure; + } + uint32_t rxpackets; + HoTTBridgeStatusRxPacketsGet(&rxpackets); + rxpackets++; + HoTTBridgeStatusRxPacketsSet(&rxpackets); + switch (message->type) { + case HoTTBRIDGEMESSAGE_PING: + ping_r_handler(m, message); + break; + case HoTTBRIDGEMESSAGE_POS_ESTIMATE: + pos_estimate_r_handler(m, message); + break; + case HoTTBRIDGEMESSAGE_FLIGHTCONTROL: + flightcontrol_r_handler(m, message); + break; + case HoTTBRIDGEMESSAGE_GIMBALCONTROL: + // TODO implement gimbal control somehow + break; + case HoTTBRIDGEMESSAGE_PONG: + pong_r_handler(m, message); + break; + default: + // do nothing at all and discard the message + break; + } + } + return; + +rxfailure: + m->rx_length = 0; + uint32_t rxfail; + HoTTBridgeStatusRxFailGet(&rxfail); + rxfail++; + HoTTBridgeStatusRxFailSet(&rxfail); +} + +static uint32_t hwsettings_rosspeed_enum_to_baud(uint8_t baud) +{ + switch (baud) { + case HWSETTINGS_HoTTSPEED_2400: + return 2400; + + case HWSETTINGS_HoTTSPEED_4800: + return 4800; + + case HWSETTINGS_HoTTSPEED_9600: + return 9600; + + case HWSETTINGS_HoTTSPEED_19200: + return 19200; + + case HWSETTINGS_HoTTSPEED_38400: + return 38400; + + case HWSETTINGS_HoTTSPEED_57600: + return 57600; + + default: + case HWSETTINGS_HoTTSPEED_115200: + return 115200; + } +} + + +/** + * Module start routine automatically called after initialization routine + * @return 0 when was successful + */ +static int32_t uavoHoTTBridgeStart(void) +{ + if (!module_enabled) { + // give port to telemetry if it doesn't have one + // stops board getting stuck in condition where it can't be connected to gcs + if (!PIOS_COM_TELEM_RF) { + PIOS_COM_TELEM_RF = PIOS_COM_HoTT; + } + + return -1; + } + + PIOS_DELTATIME_Init(&ros->roundtrip, 1e-3f, 1e-6f, 10.0f, 1e-1f); + ros->pingTimer = 0; + ros->stateTimer = 0; + ros->rateTimer = 0; + ros->rateAccumulator[0] = 0; + ros->rateAccumulator[1] = 0; + ros->rateAccumulator[2] = 0; + ros->rx_length = 0; + ros->myPingSequence = 0x66; + + xTaskHandle taskHandle; + + xTaskCreate(uavoHoTTBridgeRxTask, "uavoHoTTBridge", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_UAVOHoTTBRIDGE, taskHandle); + PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle); + + return 0; +} + +/** + * Module initialization routine + * @return 0 when initialization was successful + */ +static int32_t uavoHoTTBridgeInitialize(void) +{ + if (PIOS_COM_HoTT) { + ros = pios_malloc(sizeof(*ros)); + if (ros != NULL) { + memset(ros, 0x00, sizeof(*ros)); + + ros->com = PIOS_COM_HoTT; + + HoTTBridgeStatusInitialize(); + AUXPositionSensorInitialize(); + HwSettingsInitialize(); + HwSettingsHoTTSpeedOptions rosSpeed; + HwSettingsHoTTSpeedGet(&rosSpeed); + + PIOS_COM_ChangeBaud(PIOS_COM_HoTT, 19200); // HoTT uses 19200 only + // TODO: set COM port to 1-wire here, once PIOS_COM_ioctl is implemented + callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&uavoHoTTBridgeTxTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_UAVOHoTTBRIDGE, STACK_SIZE_BYTES); + //VelocityStateInitialize(); + //PositionStateInitialize(); + //AttitudeStateInitialize(); + //AttitudeStateConnectCallback(&AttitudeCb); + //GyroStateInitialize(); + //GyroStateConnectCallback(&RateCb); + //FlightStatusInitialize(); + //PathDesiredInitialize(); + //PoiLocationInitialize(); + //ActuatorDesiredInitialize(); + //FlightModeSettingsInitialize(); + //ManualControlCommandInitialize(); + //AccessoryDesiredInitialize(); + + module_enabled = true; + return 0; + } + } + + return -1; +} +MODULE_INITCALL(uavoHoTTBridgeInitialize, uavoHoTTBridgeStart); + +/** various handlers **/ +static void ping_r_handler(struct ros_bridge *rb, rosbridgemessage_t *m) +{ + rosbridgemessage_pingpong_t *data = (rosbridgemessage_pingpong_t *)&(m->data); + + rb->remotePingSequence = data->sequence_number; + rb->scheduled[HoTTBRIDGEMESSAGE_PONG] = true; + PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle); +} + +static void ping_handler(struct ros_bridge *rb, rosbridgemessage_t *m) +{ + rosbridgemessage_pingpong_t *data = (rosbridgemessage_pingpong_t *)&(m->data); + + data->sequence_number = ++rb->myPingSequence; + rb->roundtrip.last = PIOS_DELAY_GetRaw(); +} + +static void pong_r_handler(struct ros_bridge *rb, rosbridgemessage_t *m) +{ + rosbridgemessage_pingpong_t *data = (rosbridgemessage_pingpong_t *)&(m->data); + + if (data->sequence_number != rb->myPingSequence) { + return; + } + float roundtrip = PIOS_DELTATIME_GetAverageSeconds(&(rb->roundtrip)); + HoTTBridgeStatusPingRoundTripTimeSet(&roundtrip); +} + +static void flightcontrol_r_handler(__attribute__((unused)) struct ros_bridge *rb, rosbridgemessage_t *m) +{ + rosbridgemessage_flightcontrol_t *data = (rosbridgemessage_flightcontrol_t *)&(m->data); + FlightStatusFlightModeOptions mode; + + FlightStatusFlightModeGet(&mode); + if (mode != FLIGHTSTATUS_FLIGHTMODE_HoTTCONTROLLED) { + return; + } + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + switch (data->mode) { + case HoTTBRIDGEMESSAGE_FLIGHTCONTROL_MODE_WAYPOINT: + { + FlightModeSettingsPositionHoldOffsetData offset; + FlightModeSettingsPositionHoldOffsetGet(&offset); + pathDesired.End.North = boundf(data->control[0], settings.GeoFenceBoxMin.North, settings.GeoFenceBoxMax.North); + pathDesired.End.East = boundf(data->control[1], settings.GeoFenceBoxMin.East, settings.GeoFenceBoxMax.East); + pathDesired.End.Down = boundf(data->control[2], settings.GeoFenceBoxMin.Down, settings.GeoFenceBoxMax.Down); + pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; + pathDesired.Start.East = pathDesired.End.East; + pathDesired.Start.Down = pathDesired.End.Down; + pathDesired.StartingVelocity = 0.0f; + pathDesired.EndingVelocity = 0.0f; + pathDesired.Mode = PATHDESIRED_MODE_GOTOENDPOINT; + } + break; + case HoTTBRIDGEMESSAGE_FLIGHTCONTROL_MODE_ATTITUDE: + { + pathDesired.ModeParameters[0] = data->control[0]; + pathDesired.ModeParameters[1] = data->control[1]; + pathDesired.ModeParameters[2] = data->control[2]; + pathDesired.ModeParameters[3] = data->control[3]; + pathDesired.Mode = PATHDESIRED_MODE_FIXEDATTITUDE; + } + break; + } + PathDesiredSet(&pathDesired); + PoiLocationData poiLocation; + PoiLocationGet(&poiLocation); + poiLocation.North = data->poi[0]; + poiLocation.East = data->poi[1]; + poiLocation.Down = data->poi[2]; + PoiLocationSet(&poiLocation); +} +static void pos_estimate_r_handler(__attribute__((unused)) struct ros_bridge *rb, rosbridgemessage_t *m) +{ + rosbridgemessage_pos_estimate_t *data = (rosbridgemessage_pos_estimate_t *)&(m->data); + AUXPositionSensorData pos; + + pos.North = data->position[0]; + pos.East = data->position[1]; + pos.Down = data->position[2]; + AUXPositionSensorSet(&pos); +} + +static void pong_handler(struct ros_bridge *rb, rosbridgemessage_t *m) +{ + rosbridgemessage_pingpong_t *data = (rosbridgemessage_pingpong_t *)&(m->data); + + + data->sequence_number = rb->remotePingSequence; +} + +static void fullstate_estimate_handler(__attribute__((unused)) struct ros_bridge *rb, rosbridgemessage_t *m) +{ + PositionStateData pos; + VelocityStateData vel; + AttitudeStateData att; + FlightStatusFlightModeOptions mode; + FlightStatusArmedOptions armed; + float thrust; + AccessoryDesiredData accessory; + ManualControlCommandData manualcontrol; + + ManualControlCommandGet(&manualcontrol); + FlightStatusArmedGet(&armed); + FlightStatusFlightModeGet(&mode); + ActuatorDesiredThrustGet(&thrust); + PositionStateGet(&pos); + VelocityStateGet(&vel); + AttitudeStateGet(&att); + rosbridgemessage_fullstate_estimate_t *data = (rosbridgemessage_fullstate_estimate_t *)&(m->data); + data->quaternion[0] = att.q1; + data->quaternion[1] = att.q2; + data->quaternion[2] = att.q3; + data->quaternion[3] = att.q4; + data->position[0] = pos.North; + data->position[1] = pos.East; + data->position[2] = pos.Down; + data->velocity[0] = vel.North; + data->velocity[1] = vel.East; + data->velocity[2] = vel.Down; + data->rotation[0] = ros->rateAccumulator[0]; + data->rotation[1] = ros->rateAccumulator[1]; + data->rotation[2] = ros->rateAccumulator[2]; + data->thrust = thrust; + data->HoTTControlled = (mode == FLIGHTSTATUS_FLIGHTMODE_HoTTCONTROLLED) ? 1 : 0; + data->FlightMode = manualcontrol.FlightModeSwitchPosition; + data->armed = (armed == FLIGHTSTATUS_ARMED_ARMED) ? 1 : 0; + data->controls[0] = manualcontrol.Roll; + data->controls[1] = manualcontrol.Pitch; + data->controls[2] = manualcontrol.Yaw; + data->controls[3] = manualcontrol.Thrust; + data->controls[4] = manualcontrol.Collective; + data->controls[5] = manualcontrol.Throttle; + for (int t = 0; t < 4; t++) { + if (AccessoryDesiredInstGet(t, &accessory) == 0) { + data->accessory[t] = accessory.AccessoryVal; + } + } + + int x, y; + float *P[13]; + INSGetPAddress(P); + for (x = 0; x < 10; x++) { + for (y = 0; y < 10; y++) { + data->matrix[x * 10 + y] = P[x][y]; + } + } + if (ros->rateTimer >= 1) { + float factor = 1.0f / (float)ros->rateTimer; + data->rotation[0] *= factor; + data->rotation[1] *= factor; + data->rotation[2] *= factor; + ros->rateAccumulator[0] = 0; + ros->rateAccumulator[1] = 0; + ros->rateAccumulator[2] = 0; + ros->rateTimer = 0; + } +} +static void imu_average_handler(__attribute__((unused)) struct ros_bridge *rb, __attribute__((unused)) rosbridgemessage_t *m) +{ + // TODO +} +static void gimbal_estimate_handler(__attribute__((unused)) struct ros_bridge *rb, __attribute__((unused)) rosbridgemessage_t *m) +{ + // TODO +} + +/** + * Main task routine + * @param[in] parameters parameter given by PIOS_Callback_Create() + */ +static void uavoHoTTBridgeTxTask(void) +{ + uint8_t buffer[HoTTBRIDGEMESSAGE_BUFFERSIZE]; // buffer on the stack? could also be in static RAM but not reuseale by other callbacks then + rosbridgemessage_t *message = (rosbridgemessage_t *)buffer; + + for (rosbridgemessagetype_t type = HoTTBRIDGEMESSAGE_PING; type < HoTTBRIDGEMESSAGE_END_ARRAY_SIZE; type++) { + if (ros->scheduled[type] && rosbridgemessagehandlers[type] != NULL) { + message->magic = HoTTBRIDGEMAGIC; + message->length = HoTTBRIDGEMESSAGE_SIZES[type]; + message->type = type; + message->timestamp = PIOS_DELAY_GetuS(); + (*rosbridgemessagehandlers[type])(ros, message); + message->crc32 = PIOS_CRC32_updateCRC(0xffffffff, message->data, message->length); + int32_t ret = PIOS_COM_SendBufferNonBlocking(ros->com, buffer, offsetof(rosbridgemessage_t, data) + message->length); + // int32_t ret = PIOS_COM_SendBuffer(ros->com, buffer, offsetof(rosbridgemessage_t, data) + message->length); + ros->scheduled[type] = false; + if (ret >= 0) { + uint32_t txpackets; + HoTTBridgeStatusTxPacketsGet(&txpackets); + txpackets++; + HoTTBridgeStatusTxPacketsSet(&txpackets); + } else { + uint32_t txfail; + HoTTBridgeStatusTxFailGet(&txfail); + txfail++; + HoTTBridgeStatusTxFailSet(&txfail); + } + PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle); + return; + } + } + // nothing scheduled, do a ping in 10 secods time +} + +/** + * Event Callback on Gyro updates (called with PIOS_SENSOR_RATE - roughly 500 Hz - from State Estimation) + */ +void RateCb(__attribute__((unused)) UAVObjEvent *ev) +{ + GyroStateData gyr; + + GyroStateGet(&gyr); + ros->rateAccumulator[0] += gyr.x; + ros->rateAccumulator[1] += gyr.y; + ros->rateAccumulator[2] += gyr.z; + ros->rateTimer++; +} + +/** + * Event Callback on Attitude updates (called with PIOS_SENSOR_RATE - roughly 500 Hz - from State Estimation) + */ +void AttitudeCb(__attribute__((unused)) UAVObjEvent *ev) +{ + bool dispatch = false; + + if (++ros->pingTimer >= settings.UpdateRate.Ping && settings.UpdateRate.Ping > 0) { + ros->pingTimer = 0; + dispatch = true; + ros->scheduled[HoTTBRIDGEMESSAGE_PING] = true; + } + if (++ros->stateTimer >= settings.UpdateRate.State && settings.UpdateRate.State > 0) { + ros->stateTimer = 0; + dispatch = true; + ros->scheduled[HoTTBRIDGEMESSAGE_FULLSTATE_ESTIMATE] = true; + } + if (dispatch && callbackHandle) { + PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle); + } +} + +/** + * Main task routine + * @param[in] parameters parameter given by PIOS_Thread_Create() + */ +static void uavoHoTTBridgeRxTask(__attribute__((unused)) void *parameters) +{ + while (1) { + uint8_t b = 0; + uint16_t count = PIOS_COM_ReceiveBuffer(ros->com, &b, 1, ~0); + if (count) { + ros_receive_byte(ros, b); + } + } +} + +#endif // PIOS_INCLUDE_HoTT_BRIDGE +/** + * @} + * @} + */ From faf894c5306e1e58b5362c2f1ba12d501aef1058 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Thu, 23 Mar 2017 08:53:45 +0100 Subject: [PATCH 2/7] LP-500 Added UAVObject for HoTT bridge --- .../targets/boards/revolution/firmware/Makefile | 1 + .../boards/revolution/firmware/UAVObjects.inc | 1 + shared/uavobjectdefinition/hottbridgestatus.xml | 16 ++++++++++++++++ 3 files changed, 18 insertions(+) create mode 100644 shared/uavobjectdefinition/hottbridgestatus.xml diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index 5ee49d1a2..6a51ba5a2 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -55,6 +55,7 @@ MODULES += Notify OPTMODULES += AutoTune OPTMODULES += ComUsbBridge +OPTMODULES += UAVOHottBridge OPTMODULES += UAVOMSPBridge OPTMODULES += UAVOMavlinkBridge diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 5774e7ae4..f6d8d7987 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -126,6 +126,7 @@ UAVOBJSRCFILENAMES += poilearnsettings UAVOBJSRCFILENAMES += mpugyroaccelsettings UAVOBJSRCFILENAMES += txpidsettings UAVOBJSRCFILENAMES += txpidstatus +UAVOBJSRCFILENAMES += hottbridgestatus UAVOBJSRCFILENAMES += takeofflocation UAVOBJSRCFILENAMES += perfcounter UAVOBJSRCFILENAMES += systemidentsettings diff --git a/shared/uavobjectdefinition/hottbridgestatus.xml b/shared/uavobjectdefinition/hottbridgestatus.xml new file mode 100644 index 000000000..06fe13a45 --- /dev/null +++ b/shared/uavobjectdefinition/hottbridgestatus.xml @@ -0,0 +1,16 @@ + + + HoTTBridge Status Information + + + + + + + + + + + + + From 02b5f26a7273ccb7cec46ecd0bd5180465d3ec4b Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Thu, 23 Mar 2017 10:02:33 +0100 Subject: [PATCH 3/7] LP-500 HoTT Telemetry added device definitions --- flight/targets/boards/revolution/firmware/pios_board.c | 4 ++++ flight/targets/boards/revolution/pios_board.h | 2 ++ 2 files changed, 6 insertions(+) diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 9c42aa0ef..3470af273 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -251,6 +251,9 @@ uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; #define PIOS_COM_BRIDGE_RX_BUF_LEN 65 #define PIOS_COM_BRIDGE_TX_BUF_LEN 12 +#define PIOS_COM_HOTT_RX_BUF_LEN 512 +#define PIOS_COM_HOTT_TX_BUF_LEN 512 + #define PIOS_COM_RFM22B_RF_RX_BUF_LEN 512 #define PIOS_COM_RFM22B_RF_TX_BUF_LEN 512 @@ -272,6 +275,7 @@ uint32_t pios_com_telem_usb_id = 0; uint32_t pios_com_telem_rf_id = 0; uint32_t pios_com_rf_id = 0; uint32_t pios_com_bridge_id = 0; +uint32_t pios_com_hott_id = 0; uint32_t pios_com_overo_id = 0; uint32_t pios_com_hkosd_id = 0; uint32_t pios_com_vcp_id = 0; diff --git a/flight/targets/boards/revolution/pios_board.h b/flight/targets/boards/revolution/pios_board.h index a97fdbd67..fb0f469e8 100644 --- a/flight/targets/boards/revolution/pios_board.h +++ b/flight/targets/boards/revolution/pios_board.h @@ -146,6 +146,7 @@ extern uint32_t pios_com_rf_id; extern uint32_t pios_com_gps_id; extern uint32_t pios_com_telem_usb_id; extern uint32_t pios_com_bridge_id; +extern uint32_t pios_com_hott_id; extern uint32_t pios_com_vcp_id; extern uint32_t pios_com_hkosd_id; extern uint32_t pios_com_msp_id; @@ -156,6 +157,7 @@ extern uint32_t pios_com_mavlink_id; #define PIOS_COM_TELEM_RF (pios_com_telem_rf_id) #define PIOS_COM_RF (pios_com_rf_id) #define PIOS_COM_BRIDGE (pios_com_bridge_id) +#define PIOS_COM_HOTT (pios_com_hott_id) #define PIOS_COM_VCP (pios_com_vcp_id) #define PIOS_COM_OSDHK (pios_com_hkosd_id) #define PIOS_COM_MSP (pios_com_msp_id) From a2f833641b46470dabb4aef744b3a75324b87ab4 Mon Sep 17 00:00:00 2001 From: Eric Price Date: Thu, 23 Mar 2017 12:29:39 +0100 Subject: [PATCH 4/7] LP-500 add hottbridgestatus uavobjct to GCS --- ground/gcs/src/plugins/uavobjects/uavobjects.pro | 1 + 1 file changed, 1 insertion(+) diff --git a/ground/gcs/src/plugins/uavobjects/uavobjects.pro b/ground/gcs/src/plugins/uavobjects/uavobjects.pro index 942e79742..23bce4c0b 100644 --- a/ground/gcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/gcs/src/plugins/uavobjects/uavobjects.pro @@ -94,6 +94,7 @@ UAVOBJS = \ $${UAVOBJ_XML_DIR}/gyrosensor.xml \ $${UAVOBJ_XML_DIR}/gyrostate.xml \ $${UAVOBJ_XML_DIR}/homelocation.xml \ + $${UAVOBJ_XML_DIR}/hottbridgestatus.xml \ $${UAVOBJ_XML_DIR}/hwsettings.xml \ $${UAVOBJ_XML_DIR}/i2cstats.xml \ $${UAVOBJ_XML_DIR}/magsensor.xml \ From 545f1c863bbfc5146c6cb27eaf1ec33fb4f963f5 Mon Sep 17 00:00:00 2001 From: Eric Price Date: Fri, 24 Mar 2017 07:01:47 +0100 Subject: [PATCH 5/7] LP-500 HoTT Bridge Module ported from TauLabs --- .../UAVOHottBridge/inc/uavohottbridge.h | 379 +++++ .../modules/UAVOHottBridge/uavohottbridge.c | 1369 +++++++++++------ flight/pios/common/pios_com.c | 24 + flight/pios/inc/pios_com.h | 2 + flight/pios/stm32f4xx/pios_usart.c | 32 +- .../boards/revolution/firmware/UAVObjects.inc | 1 + .../revolution/firmware/inc/pios_config.h | 1 + .../boards/revolution/firmware/pios_board.c | 5 + .../gcs/src/plugins/uavobjects/uavobjects.pro | 1 + .../hottbridgesettings.xml | 82 + shared/uavobjectdefinition/hwsettings.xml | 4 +- shared/uavobjectdefinition/taskinfo.xml | 3 + 12 files changed, 1396 insertions(+), 507 deletions(-) create mode 100644 flight/modules/UAVOHottBridge/inc/uavohottbridge.h create mode 100644 shared/uavobjectdefinition/hottbridgesettings.xml diff --git a/flight/modules/UAVOHottBridge/inc/uavohottbridge.h b/flight/modules/UAVOHottBridge/inc/uavohottbridge.h new file mode 100644 index 000000000..470ab6304 --- /dev/null +++ b/flight/modules/UAVOHottBridge/inc/uavohottbridge.h @@ -0,0 +1,379 @@ +/** + ****************************************************************************** + * @addtogroup Modules + * @{ + * @addtogroup UAVOHoTTBridge UAVO to HoTT Bridge Telemetry Module + * @{ + * + * @file uavohottbridge.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. + * Tau Labs, http://taulabs.org, Copyright (C) 2013 + * @brief sends telemery data on HoTT request + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +// timing variables +#define IDLE_TIME 10 // idle line delay to prevent data crashes on telemetry line. +#define DATA_TIME 3 // time between 2 transmitted bytes + +// sizes and lengths +#define climbratesize 50 // defines size of ring buffer for climbrate calculation +#define statussize 21 // number of characters in status line +#define HOTT_MAX_MESSAGE_LENGTH 200 + +// scale factors +#define M_TO_CM 100 // scale m to cm or m/s to cm/s +#define MS_TO_KMH 3.6f // scale m/s to km/h +#define DEG_TO_UINT 0.5f // devide degrees by 2. then the value fits into a byte. + +// offsets. used to make transmitted values unsigned. +#define OFFSET_ALTITUDE 500 // 500m +#define OFFSET_CLIMBRATE 30000 // 30000cm/s or 300m/s +#define OFFSET_CLIMBRATE3S 120 // 120m/s +#define OFFSET_TEMPERATURE 20 // 20 degrees + +// Bits to invert display areas or values +#define VARIO_INVERT_ALT (1 << 0) // altitude +#define VARIO_INVERT_MAX (1 << 1) // max altitude +#define VARIO_INVERT_MIN (1 << 2) // min altitude +#define VARIO_INVERT_CR1S (1 << 3) // climbrate 1s +#define VARIO_INVERT_CR3S (1 << 4) // climbrate 3s +#define VARIO_INVERT_CR10S (1 << 5) // climbrate 10s + +#define GPS_INVERT_HDIST (1 << 0) // home distance +#define GPS_INVERT_SPEED (1 << 1) // speed (kmh) +#define GPS_INVERT_ALT (1 << 2) // altitude +#define GPS_INVERT_CR1S (1 << 3) // climbrate 1s +#define GPS_INVERT_CR3S (1 << 4) // climbrate 3s +#define GPS_INVERT2_POS (1 << 0) // GPS postion values + +#define GAM_INVERT_CELL (1 << 0) // cell voltage +#define GAM_INVERT_BATT1 (1 << 1) // battery 1 voltage +#define GAM_INVERT_BATT2 (1 << 2) // battery 1 voltage +#define GAM_INVERT_TEMP1 (1 << 3) // temperature 1 +#define GAM_INVERT_TEMP2 (1 << 4) // temperature 2 +#define GAM_INVERT_FUEL (1 << 5) // fuel +#define GAM_INVERT2_CURRENT (1 << 0) // current +#define GAM_INVERT2_VOLTAGE (1 << 1) // voltage +#define GAM_INVERT2_ALT (1 << 2) // altitude +#define GAM_INVERT2_CR1S (1 << 3) // climbrate 1s +#define GAM_INVERT2_CR3S (1 << 4) // climbrate 3s + +#define EAM_INVERT_CAPACITY (1 << 0) // capacity +#define EAM_INVERT_BATT1 (1 << 1) // battery 1 voltage +#define EAM_INVERT_BATT2 (1 << 2) // battery 1 voltage +#define EAM_INVERT_TEMP1 (1 << 3) // temperature 1 +#define EAM_INVERT_TEMP2 (1 << 4) // temperature 2 +#define EAM_INVERT_ALT (1 << 5) // altitude +#define EAM_INVERT_CURRENT (1 << 6) // current +#define EAM_INVERT_VOLTAGE (1 << 7) // voltage +#define EAM_INVERT2_ALT (1 << 2) // altitude +#define EAM_INVERT2_CR1S (1 << 3) // climbrate 1s +#define EAM_INVERT2_CR3S (1 << 4) // climbrate 3s + +#define ESC_INVERT_VOLTAGE (1 << 0) // voltage +#define ESC_INVERT_TEMP1 (1 << 1) // temperature 1 +#define ESC_INVERT_TEMP2 (1 << 2) // temperature 2 +#define ESC_INVERT_CURRENT (1 << 3) // current +#define ESC_INVERT_RPM (1 << 4) // rpm +#define ESC_INVERT_CAPACITY (1 << 5) // capacity +#define ESC_INVERT_MAXCURRENT (1 << 6) // maximum current + +// message codes +#define HOTT_TEXT_ID 0x7f // Text request +#define HOTT_BINARY_ID 0x80 // Binary request +#define HOTT_VARIO_ID 0x89 // Vario Module ID +#define HOTT_VARIO_TEXT_ID 0x90 // Vario Module TEXT ID +#define HOTT_GPS_ID 0x8a // GPS Module ID +#define HOTT_GPS_TEXT_ID 0xa0 // GPS Module TEXT ID +#define HOTT_ESC_ID 0x8c // ESC Module ID +#define HOTT_ESC_TEXT_ID 0xc0 // ESC Module TEXT ID +#define HOTT_GAM_ID 0x8d // General Air Module ID +#define HOTT_GAM_TEXT_ID 0xd0 // General Air Module TEXT ID +#define HOTT_EAM_ID 0x8e // Electric Air Module ID +#define HOTT_EAM_TEXT_ID 0xe0 // Electric Air Module TEXT ID +#define HOTT_TEXT_START 0x7b // Start byte Text mode +#define HOTT_START 0x7c // Start byte Binary mode +#define HOTT_STOP 0x7d // End byte +#define HOTT_BUTTON_DEC 0xEB // minus button +#define HOTT_BUTTON_INC 0xED // plus button +#define HOTT_BUTTON_SET 0xE9 // set button +#define HOTT_BUTTON_NIL 0x0F // esc button +#define HOTT_BUTTON_NEXT 0xEE // next button +#define HOTT_BUTTON_PREV 0xE7 // previous button + +// prefined signal tones or spoken announcments +#define HOTT_TONE_A 1 // minimum speed +#define HOTT_TONE_B 2 // sink rate 3 seconds +#define HOTT_TONE_C 3 // sink rate 1 second +#define HOTT_TONE_D 4 // maximum distance +#define HOTT_TONE_E 5 // - +#define HOTT_TONE_F 6 // minimum temperature sensor 1 +#define HOTT_TONE_G 7 // minimum temperature sensor 2 +#define HOTT_TONE_H 8 // maximum temperature sensor 1 +#define HOTT_TONE_I 9 // maximum temperature sensor 2 +#define HOTT_TONE_J 10 // overvoltage sensor 1 +#define HOTT_TONE_K 11 // overvoltage sensor 2 +#define HOTT_TONE_L 12 // maximum speed +#define HOTT_TONE_M 13 // climb rate 3 seconds +#define HOTT_TONE_N 14 // climb rate 1 second +#define HOTT_TONE_O 15 // minimum height +#define HOTT_TONE_P 16 // minimum input voltage +#define HOTT_TONE_Q 17 // minimum cell voltage +#define HOTT_TONE_R 18 // undervoltage sensor 1 +#define HOTT_TONE_S 19 // undervoltage sensor 2 +#define HOTT_TONE_T 20 // minimum rpm +#define HOTT_TONE_U 21 // fuel reserve +#define HOTT_TONE_V 22 // capacity +#define HOTT_TONE_W 23 // maximum current +#define HOTT_TONE_X 24 // maximum input voltage +#define HOTT_TONE_Y 25 // maximum rpm +#define HOTT_TONE_Z 26 // maximum height +#define HOTT_TONE_20M 37 // 20 meters +#define HOTT_TONE_40M 38 // 40 meters +#define HOTT_TONE_60M 39 // 60 meters +#define HOTT_TONE_80M 40 // 80 meters +#define HOTT_TONE_100M 41 // 100 meters +#define HOTT_TONE_42 42 // receiver voltage +#define HOTT_TONE_43 43 // receiver temperature +#define HOTT_TONE_200M 46 // 200 meters +#define HOTT_TONE_400M 47 // 400 meters +#define HOTT_TONE_600M 48 // 600 meters +#define HOTT_TONE_800M 49 // 800 meters +#define HOTT_TONE_1000M 50 // 10000 meters +#define HOTT_TONE_51 51 // maximum servo temperature +#define HOTT_TONE_52 52 // maximum servo position difference + + +// Private types +typedef struct { + uint8_t l; + uint8_t h; +} uword_t; + +// Private structures +struct telemetrydata { + HoTTBridgeSettingsData Settings; + AttitudeStateData Attitude; + BaroSensorData Baro; + FlightBatteryStateData Battery; + FlightStatusData FlightStatus; + GPSPositionSensorData GPS; + GPSTimeData GPStime; + GyroSensorData Gyro; + HomeLocationData Home; + PositionStateData Position; + SystemAlarmsData SysAlarms; + VelocityStateData Velocity; + int16_t climbratebuffer[climbratesize]; + uint8_t climbrate_pointer; + float altitude; + float min_altitude; + float max_altitude; + float altitude_last; + float climbrate1s; + float climbrate3s; + float climbrate10s; + float homedistance; + float homecourse; + uint8_t last_armed; + char statusline[statussize]; +}; + +// VARIO Module message structure +struct hott_vario_message { + uint8_t start; // start byte + uint8_t sensor_id; // VARIO sensor ID + uint8_t warning; // 0…= warning beeps + uint8_t sensor_text_id; // VARIO sensor text ID + uint8_t alarm_inverse; // this inverts specific parts of display + uword_t altitude; // altitude (meters), offset 500, 500 == 0m + uword_t max_altitude; // max. altitude (meters) + uword_t min_altitude; // min. altitude (meters) + uword_t climbrate; // climb rate (0.01m/s), offset 30000, 30000 == 0.00m/s + uword_t climbrate3s; // climb rate (0.01m/3s) + uword_t climbrate10s; // climb rate (0.01m/10s) + uint8_t ascii[21]; // 21 chars of text + uint8_t ascii1; // ASCII Free character [1] (next to Alt) + uint8_t ascii2; // ASCII Free character [2] (next to Dir) + uint8_t ascii3; // ASCII Free character [3] (next to I) + int8_t compass; // 1=2°, -1=-2° + uint8_t version; // version number + uint8_t stop; // stop byte + uint8_t checksum; // Lower 8-bits of all bytes summed +}; + +// GPS Module message structure +struct hott_gps_message { + uint8_t start; // start byte + uint8_t sensor_id; // GPS sensor ID + uint8_t warning; // 0…= warning beeps + uint8_t sensor_text_id; // GPS Sensor text mode ID + uint8_t alarm_inverse1; // this inverts specific parts of display + uint8_t alarm_inverse2; + uint8_t flight_direction; // flight direction (1 = 2°; 0° = north, 90° = east , 180° = south , 270° west) + uword_t gps_speed; // GPS speed (km/h) + uint8_t latitude_ns; // GPS latitude north/south (0 = N) + uword_t latitude_min; // GPS latitude (min) + uword_t latitude_sec; // GPS latitude (sec) + uint8_t longitude_ew; // GPS longitude east/west (0 = E) + uword_t longitude_min; // GPS longitude (min) + uword_t longitude_sec; // GPS longitude (sec) + uword_t distance; // distance from home location (meters) + uword_t altitude; // altitude (meters), offset 500, 500 == 0m + uword_t climbrate; // climb rate (0.01m/s), offset of 30000, 30000 = 0.00 m/s + uint8_t climbrate3s; // climb rate in (m/3s). offset of 120, 120 == 0m/3sec + uint8_t gps_num_sat; // GPS number of satelites + uint8_t gps_fix_char; // GPS FixChar ('D'=DGPS, '2'=2D, '3'=3D) + uint8_t home_direction; // home direction (1=2°, direction from starting point to model position) + int8_t angle_roll; // angle x-direction (roll 1=2°, 255=-2°) + int8_t angle_nick; // angle y-direction (nick) + int8_t angle_compass; // angle z-direction (yaw) + uint8_t gps_hour; // GPS time hours + uint8_t gps_min; // GPS time minutes + uint8_t gps_sec; // GPS time seconds + uint8_t gps_msec; // GPS time .sss + uword_t msl; // MSL or NN altitude + uint8_t vibration; // vibration + uint8_t ascii4; // ASCII Free Character [4] (next to home distance) + uint8_t ascii5; // ASCII Free Character [5] (next to home direction) + uint8_t ascii6; // ASCII Free Character [6] (next to number of sats) + uint8_t version; // version number (0=gps, 1=gyro, 255=multicopter) + uint8_t stop; // stop byte + uint8_t checksum; // Lower 8-bits of all bytes summed +}; + +// General Air Module message structure +struct hott_gam_message { + uint8_t start; // start byte + uint8_t sensor_id; // GAM sensor ID + uint8_t warning; // 0…= warning beeps + uint8_t sensor_text_id; // EAM Sensor text mode ID + uint8_t alarm_inverse1; // this inverts specific parts of display + uint8_t alarm_inverse2; + uint8_t cell1; // cell voltages in 0.02V steps, 210 == 4.2V + uint8_t cell2; + uint8_t cell3; + uint8_t cell4; + uint8_t cell5; + uint8_t cell6; + uword_t batt1_voltage; // battery sensor 1 in 0.1V steps, 50 == 5.5V + uword_t batt2_voltage; // battery sensor 2 voltage + uint8_t temperature1; // temperature 1 in °C, offset of 20, 20 == 0°C + uint8_t temperature2; // temperature 2 + uint8_t fuel_procent; // fuel capacity in %, values from 0..100 + uword_t fuel_ml; // fuel capacity in ml, values from 0..65535 + uword_t rpm; // rpm, scale factor 10, 300 == 3000rpm + uword_t altitude; // altitude in meters, offset of 500, 500 == 0m + uword_t climbrate; // climb rate (0.01m/s), offset of 30000, 30000 = 0.00 m/s + uint8_t climbrate3s; // climb rate (m/3sec). offset of 120, 120 == 0m/3sec + uword_t current; // current in 0.1A steps + uword_t voltage; // main power voltage in 0.1V steps + uword_t capacity; // used battery capacity in 10mAh steps + uword_t speed; // speed in km/h + uint8_t min_cell_volt; // lowest cell voltage in 20mV steps. 124 == 2.48V + uint8_t min_cell_volt_num; // number of the cell with the lowest voltage + uword_t rpm2; // rpm2 in 10 rpm steps, 300 == 3000rpm + uint8_t g_error_number; // general error number (Voice error == 12) + uint8_t pressure; // pressure up to 15bar, 0.1bar steps + uint8_t version; // version number + uint8_t stop; // stop byte + uint8_t checksum; // Lower 8-bits of all bytes summed +}; + +// Electric Air Module message structure +struct hott_eam_message { + uint8_t start; // Start byte + uint8_t sensor_id; // EAM sensor id + uint8_t warning; + uint8_t sensor_text_id; // EAM Sensor text mode ID + uint8_t alarm_inverse1; // this inverts specific parts of display + uint8_t alarm_inverse2; + uint8_t cell1_L; // cell voltages of the lower battery + uint8_t cell2_L; + uint8_t cell3_L; + uint8_t cell4_L; + uint8_t cell5_L; + uint8_t cell6_L; + uint8_t cell7_L; + uint8_t cell1_H; // cell voltages of higher battery + uint8_t cell2_H; + uint8_t cell3_H; + uint8_t cell4_H; + uint8_t cell5_H; + uint8_t cell6_H; + uint8_t cell7_H; + uword_t batt1_voltage; // battery sensor 1 voltage, in steps of 0.02V + uword_t batt2_voltage; // battery sensor 2 voltage, in steps of 0.02V + uint8_t temperature1; // temperature sensor 1. 20 = 0 degrees + uint8_t temperature2; // temperature sensor 2. 20 = 0 degrees + uword_t altitude; // altitude (meters). 500 = 0 meters + uword_t current; // current (A) in steps of 0.1A + uword_t voltage; // main power voltage in steps of 0.1V + uword_t capacity; // used battery capacity in steps of 10mAh + uword_t climbrate; // climb rate in 0.01m/s. 0m/s = 30000 + uint8_t climbrate3s; // climb rate in m/3sec. 0m/3sec = 120 + uword_t rpm; // rpm in steps of 10 rpm + uint8_t electric_min; // estaminated flight time in minutes. + uint8_t electric_sec; // estaminated flight time in seconds. + uword_t speed; // speed in km/h in steps of 1 km/h + uint8_t stop; // Stop byte + uint8_t checksum; // Lower 8-bits of all bytes summed. +}; + +// ESC Module message structure +struct hott_esc_message { + uint8_t start; // Start byte + uint8_t sensor_id; // EAM sensor id + uint8_t warning; + uint8_t sensor_text_id; // ESC Sensor text mode ID + uint8_t alarm_inverse1; + uint8_t alarm_inverse2; + uword_t batt_voltage; // battery voltage in steps of 0.1V + uword_t min_batt_voltage; // min battery voltage + uword_t batt_capacity; // used battery capacity in steps of 10mAh + uint8_t temperatureESC; // temperature of ESC . 20 = 0 degrees + uint8_t max_temperatureESC; // max temperature of ESC + uword_t current; // Current in steps of 0.1A + uword_t max_current; // maximal current + uword_t rpm; // rpm in steps of 10 rpm + uword_t max_rpm; // max rpm + uint8_t temperatureMOT; // motor temperature + uint8_t max_temperatureMOT; // maximal motor temperature + uint8_t dummy[19]; // 19 dummy bytes + uint8_t stop; // Stop byte + uint8_t checksum; // Lower 8-bits of all bytes summed. +}; + +// TEXT message structure +struct hott_text_message { + uint8_t start; // Start byte + uint8_t sensor_id; // TEXT id + uint8_t warning; + uint8_t text[21][8]; // text field 21 columns and 8 rows (bit 7=1 for inverse display) + uint8_t stop; // Stop byte + uint8_t checksum; // Lower 8-bits of all bytes summed. +}; + + +/** + * @} + * @} + */ diff --git a/flight/modules/UAVOHottBridge/uavohottbridge.c b/flight/modules/UAVOHottBridge/uavohottbridge.c index fdc5b0da4..56a442e36 100644 --- a/flight/modules/UAVOHottBridge/uavohottbridge.c +++ b/flight/modules/UAVOHottBridge/uavohottbridge.c @@ -2,12 +2,13 @@ ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules * @{ - * @addtogroup UAVOHoTTBridge UAVO to HoTT Bridge Module + * @addtogroup UAVOHoTTBridge UAVO to HoTT Bridge Telemetry Module * @{ * * @file uavohottridge.c * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. - * @brief Bridges certain UAVObjects to HoTT on USB VCP + * Tau Labs, http://taulabs.org, Copyright (C) 2013-2014 + * @brief sends telemery data on HoTT request * * @see The GNU Public License (GPL) Version 3 * @@ -36,213 +37,55 @@ #include "hwsettings.h" #include "taskinfo.h" #include "callbackinfo.h" -/* -#include "insgps.h" - #include "receiverstatus.h" - #include "flightmodesettings.h" - #include "flightbatterysettings.h" - #include "flightbatterystate.h" - #include "gpspositionsensor.h" - #include "manualcontrolsettings.h" - #include "oplinkstatus.h" - #include "accessorydesired.h" - #include "airspeedstate.h" - #include "actuatorsettings.h" - #include "systemstats.h" - #include "systemalarms.h" - #include "takeofflocation.h" - #include "homelocation.h" - #include "stabilizationdesired.h" - #include "stabilizationsettings.h" - #include "stabilizationbank.h" - #include "stabilizationsettingsbank1.h" - #include "stabilizationsettingsbank2.h" - #include "stabilizationsettingsbank3.h" - #include "magstate.h" -#include "manualcontrolcommand.h" -#include "accessorydesired.h" -#include "actuatordesired.h" -#include "auxpositionsensor.h" -#include "pathdesired.h" -#include "poilocation.h" -#include "flightmodesettings.h" -#include "flightstatus.h" -#include "positionstate.h" -#include "velocitystate.h" +#include "hottbridgesettings.h" #include "attitudestate.h" -#include "gyrostate.h" - */ - +#include "barosensor.h" +#include "flightbatterystate.h" +#include "flightstatus.h" +#include "gyrosensor.h" +#include "gpspositionsensor.h" +#include "gpstime.h" +#include "homelocation.h" +#include "positionstate.h" +#include "systemalarms.h" +#include "velocitystate.h" #include "hottbridgestatus.h" +#include "hottbridgesettings.h" #include "objectpersistence.h" - #include "pios_sensors.h" +#include "uavohottbridge.h" -#if defined(PIOS_INCLUDE_HoTT_BRIDGE) - -struct hott_bridge { - uintptr_t com; - - uint32_t lastPingTimestamp; - uint8_t myPingSequence; - uint8_t remotePingSequence; - PiOSDeltatimeConfig roundtrip; - double roundTripTime; - int32_t pingTimer; - int32_t stateTimer; - int32_t rateTimer; - float rateAccumulator[3]; - uint8_t rx_buffer[HoTTBRIDGEMESSAGE_BUFFERSIZE]; - size_t rx_length; - volatile bool scheduled[HoTTBRIDGEMESSAGE_END_ARRAY_SIZE]; -}; +#if defined(PIOS_INCLUDE_HOTT_BRIDGE) #if defined(PIOS_HoTT_STACK_SIZE) -#define STACK_SIZE_BYTES PIOS_HoTT_STACK_SIZE +#define STACK_SIZE_BYTES PIOS_HoTT_STACK_SIZE #else -#define STACK_SIZE_BYTES 2048 +#define STACK_SIZE_BYTES 2048 #endif -#define TASK_PRIORITY CALLBACK_TASK_AUXILIARY -#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR -#define CBTASK_PRIORITY CALLBACK_TASK_AUXILIARY +#define TASK_PRIORITY CALLBACK_TASK_AUXILIARY static bool module_enabled = false; -static struct hott_bridge *hott; -static int32_t uavoHoTTBridgeInitialize(void); -static void uavoHoTTBridgeRxTask(void *parameters); -static void uavoHoTTBridgeTxTask(void); -static DelayedCallbackInfo *callbackHandle; -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; -static HoTTBridgeSettingsData settings; -void AttitudeCb(__attribute__((unused)) UAVObjEvent *ev); -void RateCb(__attribute__((unused)) UAVObjEvent *ev); -static hottbridgemessage_handler *const hottbridgemessagehandlers[HoTTBRIDGEMESSAGE_END_ARRAY_SIZE] = { - ping_handler, - NULL, - NULL, - NULL, - pong_handler, - fullstate_estimate_handler, - imu_average_handler, - gimbal_estimate_handler -}; - - -/** - * Process incoming bytes from an HoTT query thing. - * @param[in] b received byte - * @return true if we should continue processing bytes - */ -static void hott_receive_byte(struct hott_bridge *m, uint8_t b) -{ - m->rx_buffer[m->rx_length] = b; - m->rx_length++; - hottbridgemessage_t *message = (hottbridgemessage_t *)m->rx_buffer; - - // very simple parser - but not a state machine, just a few checks - if (m->rx_length <= offsetof(hottbridgemessage_t, length)) { - // check (partial) magic number - partial is important since we need to restart at any time if garbage is received - uint32_t canary = 0xff; - for (uint32_t t = 1; t < m->rx_length; t++) { - canary = (canary << 8) || 0xff; - } - if ((message->magic & canary) != (HoTTBRIDGEMAGIC & canary)) { - // parse error, not beginning of message - goto rxfailure; - } - } - if (m->rx_length == offsetof(hottbridgemessage_t, timestamp)) { - if (message->length > (uint32_t)(HoTTBRIDGEMESSAGE_BUFFERSIZE - offsetof(hottbridgemessage_t, data))) { - // parse error, no messages are that long - goto rxfailure; - } - } - if (m->rx_length == offsetof(hottbridgemessage_t, crc32)) { - if (message->type >= HoTTBRIDGEMESSAGE_END_ARRAY_SIZE) { - // parse error - goto rxfailure; - } - if (message->length != HoTTBRIDGEMESSAGE_SIZES[message->type]) { - // parse error - goto rxfailure; - } - } - if (m->rx_length < offsetof(hottbridgemessage_t, data)) { - // not a parse failure, just not there yet - return; - } - if (m->rx_length == offsetof(hottbridgemessage_t, data) + HoTTBRIDGEMESSAGE_SIZES[message->type]) { - // complete message received and stored in pointer "message" - // empty buffer for next message - m->rx_length = 0; - - if (PIOS_CRC32_updateCRC(0xffffffff, message->data, message->length) != message->crc32) { - // crc mismatch - goto rxfailure; - } - uint32_t rxpackets; - HoTTBridgeStatusRxPacketsGet(&rxpackets); - rxpackets++; - HoTTBridgeStatusRxPacketsSet(&rxpackets); - switch (message->type) { - case HoTTBRIDGEMESSAGE_PING: - ping_r_handler(m, message); - break; - case HoTTBRIDGEMESSAGE_POS_ESTIMATE: - pos_estimate_r_handler(m, message); - break; - case HoTTBRIDGEMESSAGE_FLIGHTCONTROL: - flightcontrol_r_handler(m, message); - break; - case HoTTBRIDGEMESSAGE_GIMBALCONTROL: - // TODO implement gimbal control somehow - break; - case HoTTBRIDGEMESSAGE_PONG: - pong_r_handler(m, message); - break; - default: - // do nothing at all and discard the message - break; - } - } - return; - -rxfailure: - m->rx_length = 0; - uint32_t rxfail; - HoTTBridgeStatusRxFailGet(&rxfail); - rxfail++; - HoTTBridgeStatusRxFailSet(&rxfail); -} - -static uint32_t hwsettings_rosspeed_enum_to_baud(uint8_t baud) -{ - switch (baud) { - case HWSETTINGS_HoTTSPEED_2400: - return 2400; - - case HWSETTINGS_HoTTSPEED_4800: - return 4800; - - case HWSETTINGS_HoTTSPEED_9600: - return 9600; - - case HWSETTINGS_HoTTSPEED_19200: - return 19200; - - case HWSETTINGS_HoTTSPEED_38400: - return 38400; - - case HWSETTINGS_HoTTSPEED_57600: - return 57600; - - default: - case HWSETTINGS_HoTTSPEED_115200: - return 115200; - } -} +// Private variables +static bool module_enabled; +static struct telemetrydata *telestate; +static HoTTBridgeStatusData status; +// Private functions +static void uavoHoTTBridgeTask(void *parameters); +static uint16_t build_VARIO_message(struct hott_vario_message *msg); +static uint16_t build_GPS_message(struct hott_gps_message *msg); +static uint16_t build_GAM_message(struct hott_gam_message *msg); +static uint16_t build_EAM_message(struct hott_eam_message *msg); +static uint16_t build_ESC_message(struct hott_esc_message *msg); +static uint16_t build_TEXT_message(struct hott_text_message *msg); +static uint8_t calc_checksum(uint8_t *data, uint16_t size); +static uint8_t generate_warning(); +static void update_telemetrydata(); +static void convert_long2gps(int32_t value, uint8_t *dir, uword_t *min, uword_t *sec); +static uint8_t scale_float2uint8(float value, float scale, float offset); +static int8_t scale_float2int8(float value, float scale, float offset); +static uword_t scale_float2uword(float value, float scale, float offset); /** * Module start routine automatically called after initialization routine @@ -250,32 +93,20 @@ static uint32_t hwsettings_rosspeed_enum_to_baud(uint8_t baud) */ static int32_t uavoHoTTBridgeStart(void) { - if (!module_enabled) { - // give port to telemetry if it doesn't have one - // stops board getting stuck in condition where it can't be connected to gcs - if (!PIOS_COM_TELEM_RF) { - PIOS_COM_TELEM_RF = PIOS_COM_HoTT; - } + status.TxPackets = 0; + status.RxPackets = 0; + status.TxFail = 0; + status.RxFail = 0; - return -1; + + // Start task + if (module_enabled) { + xTaskHandle taskHandle; + + xTaskCreate(uavoHoTTBridgeTask, "uavoHoTTBridge", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_UAVOHOTTBRIDGE, taskHandle); } - PIOS_DELTATIME_Init(&ros->roundtrip, 1e-3f, 1e-6f, 10.0f, 1e-1f); - ros->pingTimer = 0; - ros->stateTimer = 0; - ros->rateTimer = 0; - ros->rateAccumulator[0] = 0; - ros->rateAccumulator[1] = 0; - ros->rateAccumulator[2] = 0; - ros->rx_length = 0; - ros->myPingSequence = 0x66; - - xTaskHandle taskHandle; - - xTaskCreate(uavoHoTTBridgeRxTask, "uavoHoTTBridge", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_UAVOHoTTBRIDGE, taskHandle); - PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle); - return 0; } @@ -285,304 +116,846 @@ static int32_t uavoHoTTBridgeStart(void) */ static int32_t uavoHoTTBridgeInitialize(void) { - if (PIOS_COM_HoTT) { - ros = pios_malloc(sizeof(*ros)); - if (ros != NULL) { - memset(ros, 0x00, sizeof(*ros)); + if (PIOS_COM_HOTT) { + module_enabled = true; + // HoTT telemetry baudrate is fixed to 19200 - ros->com = PIOS_COM_HoTT; + PIOS_COM_ChangeBaud(PIOS_COM_HOTT, 19200); + PIOS_COM_SetHalfDuplex(PIOS_COM_HOTT, true); + HoTTBridgeSettingsInitialize(); + HoTTBridgeStatusInitialize(); - HoTTBridgeStatusInitialize(); - AUXPositionSensorInitialize(); - HwSettingsInitialize(); - HwSettingsHoTTSpeedOptions rosSpeed; - HwSettingsHoTTSpeedGet(&rosSpeed); + // allocate memory for telemetry data + telestate = (struct telemetrydata *)pios_malloc(sizeof(*telestate)); - PIOS_COM_ChangeBaud(PIOS_COM_HoTT, 19200); // HoTT uses 19200 only - // TODO: set COM port to 1-wire here, once PIOS_COM_ioctl is implemented - callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&uavoHoTTBridgeTxTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_UAVOHoTTBRIDGE, STACK_SIZE_BYTES); - //VelocityStateInitialize(); - //PositionStateInitialize(); - //AttitudeStateInitialize(); - //AttitudeStateConnectCallback(&AttitudeCb); - //GyroStateInitialize(); - //GyroStateConnectCallback(&RateCb); - //FlightStatusInitialize(); - //PathDesiredInitialize(); - //PoiLocationInitialize(); - //ActuatorDesiredInitialize(); - //FlightModeSettingsInitialize(); - //ManualControlCommandInitialize(); - //AccessoryDesiredInitialize(); - - module_enabled = true; - return 0; + if (telestate == NULL) { + // there is not enough free memory. the module could not run. + module_enabled = false; + return -1; } + } else { + module_enabled = false; } - - return -1; + return 0; } MODULE_INITCALL(uavoHoTTBridgeInitialize, uavoHoTTBridgeStart); -/** various handlers **/ -static void ping_r_handler(struct ros_bridge *rb, rosbridgemessage_t *m) -{ - rosbridgemessage_pingpong_t *data = (rosbridgemessage_pingpong_t *)&(m->data); - - rb->remotePingSequence = data->sequence_number; - rb->scheduled[HoTTBRIDGEMESSAGE_PONG] = true; - PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle); -} - -static void ping_handler(struct ros_bridge *rb, rosbridgemessage_t *m) -{ - rosbridgemessage_pingpong_t *data = (rosbridgemessage_pingpong_t *)&(m->data); - - data->sequence_number = ++rb->myPingSequence; - rb->roundtrip.last = PIOS_DELAY_GetRaw(); -} - -static void pong_r_handler(struct ros_bridge *rb, rosbridgemessage_t *m) -{ - rosbridgemessage_pingpong_t *data = (rosbridgemessage_pingpong_t *)&(m->data); - - if (data->sequence_number != rb->myPingSequence) { - return; - } - float roundtrip = PIOS_DELTATIME_GetAverageSeconds(&(rb->roundtrip)); - HoTTBridgeStatusPingRoundTripTimeSet(&roundtrip); -} - -static void flightcontrol_r_handler(__attribute__((unused)) struct ros_bridge *rb, rosbridgemessage_t *m) -{ - rosbridgemessage_flightcontrol_t *data = (rosbridgemessage_flightcontrol_t *)&(m->data); - FlightStatusFlightModeOptions mode; - - FlightStatusFlightModeGet(&mode); - if (mode != FLIGHTSTATUS_FLIGHTMODE_HoTTCONTROLLED) { - return; - } - PathDesiredData pathDesired; - PathDesiredGet(&pathDesired); - switch (data->mode) { - case HoTTBRIDGEMESSAGE_FLIGHTCONTROL_MODE_WAYPOINT: - { - FlightModeSettingsPositionHoldOffsetData offset; - FlightModeSettingsPositionHoldOffsetGet(&offset); - pathDesired.End.North = boundf(data->control[0], settings.GeoFenceBoxMin.North, settings.GeoFenceBoxMax.North); - pathDesired.End.East = boundf(data->control[1], settings.GeoFenceBoxMin.East, settings.GeoFenceBoxMax.East); - pathDesired.End.Down = boundf(data->control[2], settings.GeoFenceBoxMin.Down, settings.GeoFenceBoxMax.Down); - pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; - pathDesired.Start.East = pathDesired.End.East; - pathDesired.Start.Down = pathDesired.End.Down; - pathDesired.StartingVelocity = 0.0f; - pathDesired.EndingVelocity = 0.0f; - pathDesired.Mode = PATHDESIRED_MODE_GOTOENDPOINT; - } - break; - case HoTTBRIDGEMESSAGE_FLIGHTCONTROL_MODE_ATTITUDE: - { - pathDesired.ModeParameters[0] = data->control[0]; - pathDesired.ModeParameters[1] = data->control[1]; - pathDesired.ModeParameters[2] = data->control[2]; - pathDesired.ModeParameters[3] = data->control[3]; - pathDesired.Mode = PATHDESIRED_MODE_FIXEDATTITUDE; - } - break; - } - PathDesiredSet(&pathDesired); - PoiLocationData poiLocation; - PoiLocationGet(&poiLocation); - poiLocation.North = data->poi[0]; - poiLocation.East = data->poi[1]; - poiLocation.Down = data->poi[2]; - PoiLocationSet(&poiLocation); -} -static void pos_estimate_r_handler(__attribute__((unused)) struct ros_bridge *rb, rosbridgemessage_t *m) -{ - rosbridgemessage_pos_estimate_t *data = (rosbridgemessage_pos_estimate_t *)&(m->data); - AUXPositionSensorData pos; - - pos.North = data->position[0]; - pos.East = data->position[1]; - pos.Down = data->position[2]; - AUXPositionSensorSet(&pos); -} - -static void pong_handler(struct ros_bridge *rb, rosbridgemessage_t *m) -{ - rosbridgemessage_pingpong_t *data = (rosbridgemessage_pingpong_t *)&(m->data); - - - data->sequence_number = rb->remotePingSequence; -} - -static void fullstate_estimate_handler(__attribute__((unused)) struct ros_bridge *rb, rosbridgemessage_t *m) -{ - PositionStateData pos; - VelocityStateData vel; - AttitudeStateData att; - FlightStatusFlightModeOptions mode; - FlightStatusArmedOptions armed; - float thrust; - AccessoryDesiredData accessory; - ManualControlCommandData manualcontrol; - - ManualControlCommandGet(&manualcontrol); - FlightStatusArmedGet(&armed); - FlightStatusFlightModeGet(&mode); - ActuatorDesiredThrustGet(&thrust); - PositionStateGet(&pos); - VelocityStateGet(&vel); - AttitudeStateGet(&att); - rosbridgemessage_fullstate_estimate_t *data = (rosbridgemessage_fullstate_estimate_t *)&(m->data); - data->quaternion[0] = att.q1; - data->quaternion[1] = att.q2; - data->quaternion[2] = att.q3; - data->quaternion[3] = att.q4; - data->position[0] = pos.North; - data->position[1] = pos.East; - data->position[2] = pos.Down; - data->velocity[0] = vel.North; - data->velocity[1] = vel.East; - data->velocity[2] = vel.Down; - data->rotation[0] = ros->rateAccumulator[0]; - data->rotation[1] = ros->rateAccumulator[1]; - data->rotation[2] = ros->rateAccumulator[2]; - data->thrust = thrust; - data->HoTTControlled = (mode == FLIGHTSTATUS_FLIGHTMODE_HoTTCONTROLLED) ? 1 : 0; - data->FlightMode = manualcontrol.FlightModeSwitchPosition; - data->armed = (armed == FLIGHTSTATUS_ARMED_ARMED) ? 1 : 0; - data->controls[0] = manualcontrol.Roll; - data->controls[1] = manualcontrol.Pitch; - data->controls[2] = manualcontrol.Yaw; - data->controls[3] = manualcontrol.Thrust; - data->controls[4] = manualcontrol.Collective; - data->controls[5] = manualcontrol.Throttle; - for (int t = 0; t < 4; t++) { - if (AccessoryDesiredInstGet(t, &accessory) == 0) { - data->accessory[t] = accessory.AccessoryVal; - } - } - - int x, y; - float *P[13]; - INSGetPAddress(P); - for (x = 0; x < 10; x++) { - for (y = 0; y < 10; y++) { - data->matrix[x * 10 + y] = P[x][y]; - } - } - if (ros->rateTimer >= 1) { - float factor = 1.0f / (float)ros->rateTimer; - data->rotation[0] *= factor; - data->rotation[1] *= factor; - data->rotation[2] *= factor; - ros->rateAccumulator[0] = 0; - ros->rateAccumulator[1] = 0; - ros->rateAccumulator[2] = 0; - ros->rateTimer = 0; - } -} -static void imu_average_handler(__attribute__((unused)) struct ros_bridge *rb, __attribute__((unused)) rosbridgemessage_t *m) -{ - // TODO -} -static void gimbal_estimate_handler(__attribute__((unused)) struct ros_bridge *rb, __attribute__((unused)) rosbridgemessage_t *m) -{ - // TODO -} - /** - * Main task routine - * @param[in] parameters parameter given by PIOS_Callback_Create() + * Main task. It does not return. */ -static void uavoHoTTBridgeTxTask(void) +static void uavoHoTTBridgeTask(__attribute__((unused)) void *parameters) { - uint8_t buffer[HoTTBRIDGEMESSAGE_BUFFERSIZE]; // buffer on the stack? could also be in static RAM but not reuseale by other callbacks then - rosbridgemessage_t *message = (rosbridgemessage_t *)buffer; + uint8_t rx_buffer[2]; + uint8_t tx_buffer[HOTT_MAX_MESSAGE_LENGTH]; + uint16_t message_size; - for (rosbridgemessagetype_t type = HoTTBRIDGEMESSAGE_PING; type < HoTTBRIDGEMESSAGE_END_ARRAY_SIZE; type++) { - if (ros->scheduled[type] && rosbridgemessagehandlers[type] != NULL) { - message->magic = HoTTBRIDGEMAGIC; - message->length = HoTTBRIDGEMESSAGE_SIZES[type]; - message->type = type; - message->timestamp = PIOS_DELAY_GetuS(); - (*rosbridgemessagehandlers[type])(ros, message); - message->crc32 = PIOS_CRC32_updateCRC(0xffffffff, message->data, message->length); - int32_t ret = PIOS_COM_SendBufferNonBlocking(ros->com, buffer, offsetof(rosbridgemessage_t, data) + message->length); - // int32_t ret = PIOS_COM_SendBuffer(ros->com, buffer, offsetof(rosbridgemessage_t, data) + message->length); - ros->scheduled[type] = false; - if (ret >= 0) { - uint32_t txpackets; - HoTTBridgeStatusTxPacketsGet(&txpackets); - txpackets++; - HoTTBridgeStatusTxPacketsSet(&txpackets); - } else { - uint32_t txfail; - HoTTBridgeStatusTxFailGet(&txfail); - txfail++; - HoTTBridgeStatusTxFailSet(&txfail); - } - PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle); - return; - } - } - // nothing scheduled, do a ping in 10 secods time -} + // clear all state values + memset(telestate, 0, sizeof(*telestate)); -/** - * Event Callback on Gyro updates (called with PIOS_SENSOR_RATE - roughly 500 Hz - from State Estimation) - */ -void RateCb(__attribute__((unused)) UAVObjEvent *ev) -{ - GyroStateData gyr; + // initialize timer variables + portTickType lastSysTime = xTaskGetTickCount(); + // idle delay between telemetry request and answer + uint32_t idledelay = IDLE_TIME; + // data delay between transmitted bytes + uint32_t datadelay = DATA_TIME; - GyroStateGet(&gyr); - ros->rateAccumulator[0] += gyr.x; - ros->rateAccumulator[1] += gyr.y; - ros->rateAccumulator[2] += gyr.z; - ros->rateTimer++; -} - -/** - * Event Callback on Attitude updates (called with PIOS_SENSOR_RATE - roughly 500 Hz - from State Estimation) - */ -void AttitudeCb(__attribute__((unused)) UAVObjEvent *ev) -{ - bool dispatch = false; - - if (++ros->pingTimer >= settings.UpdateRate.Ping && settings.UpdateRate.Ping > 0) { - ros->pingTimer = 0; - dispatch = true; - ros->scheduled[HoTTBRIDGEMESSAGE_PING] = true; - } - if (++ros->stateTimer >= settings.UpdateRate.State && settings.UpdateRate.State > 0) { - ros->stateTimer = 0; - dispatch = true; - ros->scheduled[HoTTBRIDGEMESSAGE_FULLSTATE_ESTIMATE] = true; - } - if (dispatch && callbackHandle) { - PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle); - } -} - -/** - * Main task routine - * @param[in] parameters parameter given by PIOS_Thread_Create() - */ -static void uavoHoTTBridgeRxTask(__attribute__((unused)) void *parameters) -{ + // work on hott telemetry. endless loop. while (1) { - uint8_t b = 0; - uint16_t count = PIOS_COM_ReceiveBuffer(ros->com, &b, 1, ~0); - if (count) { - ros_receive_byte(ros, b); + // clear message size on every loop before processing + message_size = 0; + + // shift receiver buffer. make room for one byte. + rx_buffer[1] = rx_buffer[0]; + + // wait for a byte of telemetry request in data delay interval + while (PIOS_COM_ReceiveBuffer(PIOS_COM_HOTT, rx_buffer, 1, 0) == 0) { + vTaskDelayUntil(&lastSysTime, datadelay / portTICK_RATE_MS); + } + // set start trigger point + lastSysTime = xTaskGetTickCount(); + + // examine received data stream + if (rx_buffer[1] == HOTT_BINARY_ID) { + // first received byte looks like a binary request. check second received byte for a sensor id. + switch (rx_buffer[0]) { + case HOTT_VARIO_ID: + message_size = build_VARIO_message((struct hott_vario_message *)tx_buffer); + break; + case HOTT_GPS_ID: + message_size = build_GPS_message((struct hott_gps_message *)tx_buffer); + break; + case HOTT_GAM_ID: + message_size = build_GAM_message((struct hott_gam_message *)tx_buffer); + break; + case HOTT_EAM_ID: + message_size = build_EAM_message((struct hott_eam_message *)tx_buffer); + break; + case HOTT_ESC_ID: + message_size = build_ESC_message((struct hott_esc_message *)tx_buffer); + break; + default: + message_size = 0; + } + } else if (rx_buffer[1] == HOTT_TEXT_ID) { + // first received byte looks like a text request. check second received byte for a valid button. + switch (rx_buffer[0]) { + case HOTT_BUTTON_DEC: + case HOTT_BUTTON_INC: + case HOTT_BUTTON_SET: + case HOTT_BUTTON_NIL: + case HOTT_BUTTON_NEXT: + case HOTT_BUTTON_PREV: + message_size = build_TEXT_message((struct hott_text_message *)tx_buffer); + break; + default: + message_size = 0; + } + } + + // check if a message is in the transmit buffer. + if (message_size > 0) { + status.RxPackets++; + + // check idle line before transmit. pause, then check receiver buffer + vTaskDelayUntil(&lastSysTime, idledelay / portTICK_RATE_MS); + + if (PIOS_COM_ReceiveBuffer(PIOS_COM_HOTT, rx_buffer, 1, 0) == 0) { + // nothing received means idle line. ready to transmit the requested message + for (int i = 0; i < message_size; i++) { + // send message content with pause between each byte + PIOS_COM_SendCharNonBlocking(PIOS_COM_HOTT, tx_buffer[i]); + // grab possible incoming loopback data and throw it away + PIOS_COM_ReceiveBuffer(PIOS_COM_HOTT, rx_buffer, sizeof(rx_buffer), 0); + vTaskDelayUntil(&lastSysTime, datadelay / portTICK_RATE_MS); + } + status.TxPackets++; + + + // after transmitting the message, any loopback data needs to be cleaned up. + vTaskDelayUntil(&lastSysTime, idledelay / portTICK_RATE_MS); + PIOS_COM_ReceiveBuffer(PIOS_COM_HOTT, tx_buffer, message_size, 0); + } else { + status.RxFail++; + } + HoTTBridgeStatusSet(&status); } } } -#endif // PIOS_INCLUDE_HoTT_BRIDGE +/** + * Build requested answer messages. + * \return value sets message size + */ +uint16_t build_VARIO_message(struct hott_vario_message *msg) +{ + update_telemetrydata(); + + if (telestate->Settings.Sensor.VARIO == HOTTBRIDGESETTINGS_SENSOR_DISABLED) { + return 0; + } + + // clear message buffer + memset(msg, 0, sizeof(*msg)); + + // message header + msg->start = HOTT_START; + msg->stop = HOTT_STOP; + msg->sensor_id = HOTT_VARIO_ID; + msg->warning = generate_warning(); + msg->sensor_text_id = HOTT_VARIO_TEXT_ID; + + // alarm inverse bits. invert display areas on limits + msg->alarm_inverse |= (telestate->Settings.Limit.MinHeight > telestate->altitude) ? VARIO_INVERT_ALT : 0; + msg->alarm_inverse |= (telestate->Settings.Limit.MaxHeight < telestate->altitude) ? VARIO_INVERT_ALT : 0; + msg->alarm_inverse |= (telestate->Settings.Limit.MaxHeight < telestate->altitude) ? VARIO_INVERT_MAX : 0; + msg->alarm_inverse |= (telestate->Settings.Limit.MinHeight > telestate->altitude) ? VARIO_INVERT_MIN : 0; + msg->alarm_inverse |= (telestate->Settings.Limit.NegDifference1 > telestate->climbrate1s) ? VARIO_INVERT_CR1S : 0; + msg->alarm_inverse |= (telestate->Settings.Limit.PosDifference1 < telestate->climbrate1s) ? VARIO_INVERT_CR1S : 0; + msg->alarm_inverse |= (telestate->Settings.Limit.NegDifference2 > telestate->climbrate3s) ? VARIO_INVERT_CR3S : 0; + msg->alarm_inverse |= (telestate->Settings.Limit.PosDifference2 < telestate->climbrate3s) ? VARIO_INVERT_CR3S : 0; + msg->alarm_inverse |= (telestate->Settings.Limit.NegDifference2 > telestate->climbrate10s) ? VARIO_INVERT_CR10S : 0; + msg->alarm_inverse |= (telestate->Settings.Limit.PosDifference2 < telestate->climbrate10s) ? VARIO_INVERT_CR10S : 0; + + // altitude relative to ground + msg->altitude = scale_float2uword(telestate->altitude, 1, OFFSET_ALTITUDE); + msg->min_altitude = scale_float2uword(telestate->min_altitude, 1, OFFSET_ALTITUDE); + msg->max_altitude = scale_float2uword(telestate->max_altitude, 1, OFFSET_ALTITUDE); + + // climbrate + msg->climbrate = scale_float2uword(telestate->climbrate1s, M_TO_CM, OFFSET_CLIMBRATE); + msg->climbrate3s = scale_float2uword(telestate->climbrate3s, M_TO_CM, OFFSET_CLIMBRATE); + msg->climbrate10s = scale_float2uword(telestate->climbrate10s, M_TO_CM, OFFSET_CLIMBRATE); + + // compass + msg->compass = scale_float2int8(telestate->Attitude.Yaw, DEG_TO_UINT, 0); + + // statusline + memcpy(msg->ascii, telestate->statusline, sizeof(msg->ascii)); + + // free display characters + msg->ascii1 = 0; + msg->ascii2 = 0; + msg->ascii3 = 0; + + msg->checksum = calc_checksum((uint8_t *)msg, sizeof(*msg)); + return sizeof(*msg); +} + +uint16_t build_GPS_message(struct hott_gps_message *msg) +{ + update_telemetrydata(); + + if (telestate->Settings.Sensor.GPS == HOTTBRIDGESETTINGS_SENSOR_DISABLED) { + return 0; + } + + // clear message buffer + memset(msg, 0, sizeof(*msg)); + + // message header + msg->start = HOTT_START; + msg->stop = HOTT_STOP; + msg->sensor_id = HOTT_GPS_ID; + msg->warning = generate_warning(); + msg->sensor_text_id = HOTT_GPS_TEXT_ID; + + // alarm inverse bits. invert display areas on limits + msg->alarm_inverse1 |= (telestate->Settings.Limit.MaxDistance < telestate->homedistance) ? GPS_INVERT_HDIST : 0; + msg->alarm_inverse1 |= (telestate->Settings.Limit.MinSpeed > telestate->GPS.Groundspeed) ? GPS_INVERT_SPEED : 0; + msg->alarm_inverse1 |= (telestate->Settings.Limit.MaxSpeed < telestate->GPS.Groundspeed) ? GPS_INVERT_SPEED : 0; + msg->alarm_inverse1 |= (telestate->Settings.Limit.MinHeight > telestate->altitude) ? GPS_INVERT_ALT : 0; + msg->alarm_inverse1 |= (telestate->Settings.Limit.MaxHeight < telestate->altitude) ? GPS_INVERT_ALT : 0; + msg->alarm_inverse1 |= (telestate->Settings.Limit.NegDifference1 > telestate->climbrate1s) ? GPS_INVERT_CR1S : 0; + msg->alarm_inverse1 |= (telestate->Settings.Limit.PosDifference1 < telestate->climbrate1s) ? GPS_INVERT_CR1S : 0; + msg->alarm_inverse1 |= (telestate->Settings.Limit.NegDifference2 > telestate->climbrate3s) ? GPS_INVERT_CR3S : 0; + msg->alarm_inverse1 |= (telestate->Settings.Limit.PosDifference2 < telestate->climbrate3s) ? GPS_INVERT_CR3S : 0; + msg->alarm_inverse2 |= (telestate->SysAlarms.Alarm.GPS != SYSTEMALARMS_ALARM_OK) ? GPS_INVERT2_POS : 0; + + // gps direction, groundspeed and postition + msg->flight_direction = scale_float2uint8(telestate->GPS.Heading, DEG_TO_UINT, 0); + msg->gps_speed = scale_float2uword(telestate->GPS.Groundspeed, MS_TO_KMH, 0); + convert_long2gps(telestate->GPS.Latitude, &msg->latitude_ns, &msg->latitude_min, &msg->latitude_sec); + convert_long2gps(telestate->GPS.Longitude, &msg->longitude_ew, &msg->longitude_min, &msg->longitude_sec); + + // homelocation distance, course and state + msg->distance = scale_float2uword(telestate->homedistance, 1, 0); + msg->home_direction = scale_float2uint8(telestate->homecourse, DEG_TO_UINT, 0); + msg->ascii5 = (telestate->Home.Set ? 'H' : '-'); + + // altitude relative to ground and climb rate + msg->altitude = scale_float2uword(telestate->altitude, 1, OFFSET_ALTITUDE); + msg->climbrate = scale_float2uword(telestate->climbrate1s, M_TO_CM, OFFSET_CLIMBRATE); + msg->climbrate3s = scale_float2uint8(telestate->climbrate3s, 1, OFFSET_CLIMBRATE3S); + + // number of satellites,gps fix and state + msg->gps_num_sat = telestate->GPS.Satellites; + switch (telestate->GPS.Status) { + case GPSPOSITIONSENSOR_STATUS_FIX2D: + msg->gps_fix_char = '2'; + break; + case GPSPOSITIONSENSOR_STATUS_FIX3D: + msg->gps_fix_char = '3'; + break; + default: + msg->gps_fix_char = 0; + } + switch (telestate->SysAlarms.Alarm.GPS) { + case SYSTEMALARMS_ALARM_UNINITIALISED: + msg->ascii6 = 0; + // if there is no gps, show compass flight direction + msg->flight_direction = scale_float2int8((telestate->Attitude.Yaw > 0) ? telestate->Attitude.Yaw : 360 + telestate->Attitude.Yaw, DEG_TO_UINT, 0); + break; + case SYSTEMALARMS_ALARM_OK: + msg->ascii6 = '.'; + break; + case SYSTEMALARMS_ALARM_WARNING: + msg->ascii6 = '?'; + break; + case SYSTEMALARMS_ALARM_ERROR: + case SYSTEMALARMS_ALARM_CRITICAL: + msg->ascii6 = '!'; + break; + default: + msg->ascii6 = 0; + } + + // model angles + msg->angle_roll = scale_float2int8(telestate->Attitude.Roll, DEG_TO_UINT, 0); + msg->angle_nick = scale_float2int8(telestate->Attitude.Pitch, DEG_TO_UINT, 0); + msg->angle_compass = scale_float2int8(telestate->Attitude.Yaw, DEG_TO_UINT, 0); + + // gps time + msg->gps_hour = telestate->GPStime.Hour; + msg->gps_min = telestate->GPStime.Minute; + msg->gps_sec = telestate->GPStime.Second; + msg->gps_msec = 0; + + // gps MSL (NN) altitude MSL + msg->msl = scale_float2uword(telestate->GPS.Altitude, 1, 0); + + // free display chararacter + msg->ascii4 = 0; + + msg->checksum = calc_checksum((uint8_t *)msg, sizeof(*msg)); + return sizeof(*msg); +} + +uint16_t build_GAM_message(struct hott_gam_message *msg) +{ + update_telemetrydata(); + + if (telestate->Settings.Sensor.GAM == HOTTBRIDGESETTINGS_SENSOR_DISABLED) { + return 0; + } + + // clear message buffer + memset(msg, 0, sizeof(*msg)); + + // message header + msg->start = HOTT_START; + msg->stop = HOTT_STOP; + msg->sensor_id = HOTT_GAM_ID; + msg->warning = generate_warning(); + msg->sensor_text_id = HOTT_GAM_TEXT_ID; + + // alarm inverse bits. invert display areas on limits + msg->alarm_inverse2 |= (telestate->Settings.Limit.MaxCurrent < telestate->Battery.Current) ? GAM_INVERT2_CURRENT : 0; + msg->alarm_inverse2 |= (telestate->Settings.Limit.MinPowerVoltage > telestate->Battery.Voltage) ? GAM_INVERT2_VOLTAGE : 0; + msg->alarm_inverse2 |= (telestate->Settings.Limit.MaxPowerVoltage < telestate->Battery.Voltage) ? GAM_INVERT2_VOLTAGE : 0; + msg->alarm_inverse2 |= (telestate->Settings.Limit.MinHeight > telestate->altitude) ? GAM_INVERT2_ALT : 0; + msg->alarm_inverse2 |= (telestate->Settings.Limit.MaxHeight < telestate->altitude) ? GAM_INVERT2_ALT : 0; + msg->alarm_inverse2 |= (telestate->Settings.Limit.NegDifference1 > telestate->climbrate1s) ? GAM_INVERT2_CR1S : 0; + msg->alarm_inverse2 |= (telestate->Settings.Limit.PosDifference1 < telestate->climbrate1s) ? GAM_INVERT2_CR1S : 0; + msg->alarm_inverse2 |= (telestate->Settings.Limit.NegDifference2 > telestate->climbrate3s) ? GAM_INVERT2_CR3S : 0; + msg->alarm_inverse2 |= (telestate->Settings.Limit.PosDifference2 < telestate->climbrate3s) ? GAM_INVERT2_CR3S : 0; + + // temperatures + msg->temperature1 = scale_float2uint8(telestate->Gyro.temperature, 1, OFFSET_TEMPERATURE); + msg->temperature2 = scale_float2uint8(telestate->Baro.Temperature, 1, OFFSET_TEMPERATURE); + + // altitude + msg->altitude = scale_float2uword(telestate->altitude, 1, OFFSET_ALTITUDE); + + // climbrate + msg->climbrate = scale_float2uword(telestate->climbrate1s, M_TO_CM, OFFSET_CLIMBRATE); + msg->climbrate3s = scale_float2uint8(telestate->climbrate3s, 1, OFFSET_CLIMBRATE3S); + + // main battery + float voltage = (telestate->Battery.Voltage > 0) ? telestate->Battery.Voltage : 0; + float current = (telestate->Battery.Current > 0) ? telestate->Battery.Current : 0; + float energy = (telestate->Battery.ConsumedEnergy > 0) ? telestate->Battery.ConsumedEnergy : 0; + msg->voltage = scale_float2uword(voltage, 10, 0); + msg->current = scale_float2uword(current, 10, 0); + msg->capacity = scale_float2uword(energy, 0.1f, 0); + + // pressure kPa to 0.1Bar + msg->pressure = scale_float2uint8(telestate->Baro.Pressure, 0.1f, 0); + + msg->checksum = calc_checksum((uint8_t *)msg, sizeof(*msg)); + return sizeof(*msg); +} + +uint16_t build_EAM_message(struct hott_eam_message *msg) +{ + update_telemetrydata(); + + if (telestate->Settings.Sensor.EAM == HOTTBRIDGESETTINGS_SENSOR_DISABLED) { + return 0; + } + + // clear message buffer + memset(msg, 0, sizeof(*msg)); + + // message header + msg->start = HOTT_START; + msg->stop = HOTT_STOP; + msg->sensor_id = HOTT_EAM_ID; + msg->warning = generate_warning(); + msg->sensor_text_id = HOTT_EAM_TEXT_ID; + + // alarm inverse bits. invert display areas on limits + msg->alarm_inverse1 |= (telestate->Settings.Limit.MaxUsedCapacity < telestate->Battery.ConsumedEnergy) ? EAM_INVERT_CAPACITY : 0; + msg->alarm_inverse1 |= (telestate->Settings.Limit.MaxCurrent < telestate->Battery.Current) ? EAM_INVERT_CURRENT : 0; + msg->alarm_inverse1 |= (telestate->Settings.Limit.MinPowerVoltage > telestate->Battery.Voltage) ? EAM_INVERT_VOLTAGE : 0; + msg->alarm_inverse1 |= (telestate->Settings.Limit.MaxPowerVoltage < telestate->Battery.Voltage) ? EAM_INVERT_VOLTAGE : 0; + msg->alarm_inverse2 |= (telestate->Settings.Limit.MinHeight > telestate->altitude) ? EAM_INVERT2_ALT : 0; + msg->alarm_inverse2 |= (telestate->Settings.Limit.MaxHeight < telestate->altitude) ? EAM_INVERT2_ALT : 0; + msg->alarm_inverse2 |= (telestate->Settings.Limit.NegDifference1 > telestate->climbrate1s) ? EAM_INVERT2_CR1S : 0; + msg->alarm_inverse2 |= (telestate->Settings.Limit.PosDifference1 < telestate->climbrate1s) ? EAM_INVERT2_CR1S : 0; + msg->alarm_inverse2 |= (telestate->Settings.Limit.NegDifference2 > telestate->climbrate3s) ? EAM_INVERT2_CR3S : 0; + msg->alarm_inverse2 |= (telestate->Settings.Limit.PosDifference2 < telestate->climbrate3s) ? EAM_INVERT2_CR3S : 0; + + // main battery + float voltage = (telestate->Battery.Voltage > 0) ? telestate->Battery.Voltage : 0; + float current = (telestate->Battery.Current > 0) ? telestate->Battery.Current : 0; + float energy = (telestate->Battery.ConsumedEnergy > 0) ? telestate->Battery.ConsumedEnergy : 0; + msg->voltage = scale_float2uword(voltage, 10, 0); + msg->current = scale_float2uword(current, 10, 0); + msg->capacity = scale_float2uword(energy, 0.1f, 0); + + // temperatures + msg->temperature1 = scale_float2uint8(telestate->Gyro.temperature, 1, OFFSET_TEMPERATURE); + msg->temperature2 = scale_float2uint8(telestate->Baro.Temperature, 1, OFFSET_TEMPERATURE); + + // altitude + msg->altitude = scale_float2uword(telestate->altitude, 1, OFFSET_ALTITUDE); + + // climbrate + msg->climbrate = scale_float2uword(telestate->climbrate1s, M_TO_CM, OFFSET_CLIMBRATE); + msg->climbrate3s = scale_float2uint8(telestate->climbrate3s, 1, OFFSET_CLIMBRATE3S); + + // flight time + float flighttime = (telestate->Battery.EstimatedFlightTime <= 5999) ? telestate->Battery.EstimatedFlightTime : 5999; + msg->electric_min = flighttime / 60; + msg->electric_sec = flighttime - 60 * msg->electric_min; + + msg->checksum = calc_checksum((uint8_t *)msg, sizeof(*msg)); + return sizeof(*msg); +} + +uint16_t build_ESC_message(struct hott_esc_message *msg) +{ + update_telemetrydata(); + + if (telestate->Settings.Sensor.ESC == HOTTBRIDGESETTINGS_SENSOR_DISABLED) { + return 0; + } + + // clear message buffer + memset(msg, 0, sizeof(*msg)); + + // message header + msg->start = HOTT_START; + msg->stop = HOTT_STOP; + msg->sensor_id = HOTT_ESC_ID; + msg->warning = 0; + msg->sensor_text_id = HOTT_ESC_TEXT_ID; + + // main batterie + float voltage = (telestate->Battery.Voltage > 0) ? telestate->Battery.Voltage : 0; + float current = (telestate->Battery.Current > 0) ? telestate->Battery.Current : 0; + float max_current = (telestate->Battery.PeakCurrent > 0) ? telestate->Battery.PeakCurrent : 0; + float energy = (telestate->Battery.ConsumedEnergy > 0) ? telestate->Battery.ConsumedEnergy : 0; + msg->batt_voltage = scale_float2uword(voltage, 10, 0); + msg->current = scale_float2uword(current, 10, 0); + msg->max_current = scale_float2uword(max_current, 10, 0); + msg->batt_capacity = scale_float2uword(energy, 0.1f, 0); + + // temperatures + msg->temperatureESC = scale_float2uint8(telestate->Gyro.temperature, 1, OFFSET_TEMPERATURE); + msg->max_temperatureESC = scale_float2uint8(0, 1, OFFSET_TEMPERATURE); + msg->temperatureMOT = scale_float2uint8(telestate->Baro.Temperature, 1, OFFSET_TEMPERATURE); + msg->max_temperatureMOT = scale_float2uint8(0, 1, OFFSET_TEMPERATURE); + + msg->checksum = calc_checksum((uint8_t *)msg, sizeof(*msg)); + return sizeof(*msg); +} + +uint16_t build_TEXT_message(struct hott_text_message *msg) +{ + update_telemetrydata(); + + // clear message buffer + memset(msg, 0, sizeof(*msg)); + + // message header + msg->start = HOTT_START; + msg->stop = HOTT_STOP; + msg->sensor_id = HOTT_TEXT_ID; + + msg->checksum = calc_checksum((uint8_t *)msg, sizeof(*msg)); + return sizeof(*msg); +} + +/** + * update telemetry data + * this is called on every telemetry request + * calling interval is 200ms depending on TX + * 200ms telemetry request is used as time base for timed calculations (5Hz interval) + */ +void update_telemetrydata() +{ + // update all available data + if (HoTTBridgeSettingsHandle() != NULL) { + HoTTBridgeSettingsGet(&telestate->Settings); + } + if (AttitudeStateHandle() != NULL) { + AttitudeStateGet(&telestate->Attitude); + } + if (BaroSensorHandle() != NULL) { + BaroSensorGet(&telestate->Baro); + } + if (FlightBatteryStateHandle() != NULL) { + FlightBatteryStateGet(&telestate->Battery); + } + if (FlightStatusHandle() != NULL) { + FlightStatusGet(&telestate->FlightStatus); + } + if (GPSPositionSensorHandle() != NULL) { + GPSPositionSensorGet(&telestate->GPS); + } + if (GPSTimeHandle() != NULL) { + GPSTimeGet(&telestate->GPStime); + } + if (GyroSensorHandle() != NULL) { + GyroSensorGet(&telestate->Gyro); + } + if (HomeLocationHandle() != NULL) { + HomeLocationGet(&telestate->Home); + } + if (PositionStateHandle() != NULL) { + PositionStateGet(&telestate->Position); + } + if (SystemAlarmsHandle() != NULL) { + SystemAlarmsGet(&telestate->SysAlarms); + } + if (VelocityStateHandle() != NULL) { + VelocityStateGet(&telestate->Velocity); + } + + // send actual climbrate value to ring buffer as mm per 0.2s values + uint8_t n = telestate->climbrate_pointer; + telestate->climbratebuffer[telestate->climbrate_pointer++] = -telestate->Velocity.Down * 200; + telestate->climbrate_pointer %= climbratesize; + + // calculate avarage climbrates in meters per 1, 3 and 10 second(s) based on 200ms interval + telestate->climbrate1s = 0; + telestate->climbrate3s = 0; + telestate->climbrate10s = 0; + for (uint8_t i = 0; i < climbratesize; i++) { + telestate->climbrate1s += (i < 5) ? telestate->climbratebuffer[n] : 0; + telestate->climbrate3s += (i < 15) ? telestate->climbratebuffer[n] : 0; + telestate->climbrate10s += (i < 50) ? telestate->climbratebuffer[n] : 0; + n += climbratesize - 1; + n %= climbratesize; + } + telestate->climbrate1s = telestate->climbrate1s / 1000; + telestate->climbrate3s = telestate->climbrate3s / 1000; + telestate->climbrate10s = telestate->climbrate10s / 1000; + + // set altitude offset and clear min/max values when arming + if ((telestate->FlightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING) || ((telestate->last_armed != FLIGHTSTATUS_ARMED_ARMED) && (telestate->FlightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED))) { + telestate->min_altitude = 0; + telestate->max_altitude = 0; + } + telestate->last_armed = telestate->FlightStatus.Armed; + + // calculate altitude relative to start position + telestate->altitude = -telestate->Position.Down; + + // check and set min/max values when armed. + if (telestate->FlightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { + if (telestate->min_altitude > telestate->altitude) { + telestate->min_altitude = telestate->altitude; + } + if (telestate->max_altitude < telestate->altitude) { + telestate->max_altitude = telestate->altitude; + } + } + + // gps home position and course + telestate->homedistance = sqrtf(telestate->Position.North * telestate->Position.North + telestate->Position.East * telestate->Position.East); + telestate->homecourse = acosf(-telestate->Position.North / telestate->homedistance) / 3.14159265f * 180; + if (telestate->Position.East > 0) { + telestate->homecourse = 360 - telestate->homecourse; + } + + // statusline + const char *txt_unknown = "unknown"; + const char *txt_manual = "Manual"; + const char *txt_stabilized1 = "Stabilized1"; + const char *txt_stabilized2 = "Stabilized2"; + const char *txt_stabilized3 = "Stabilized3"; + const char *txt_stabilized4 = "Stabilized4"; + const char *txt_stabilized5 = "Stabilized5"; + const char *txt_stabilized6 = "Stabilized6"; + const char *txt_positionhold = "PositionHold"; + const char *txt_courselock = "CourseLock"; + const char *txt_velocityroam = "VelocityRoam"; + const char *txt_homeleash = "HomeLeash"; + const char *txt_absoluteposition = "AbsolutePosition"; + const char *txt_returntobase = "ReturnToBase"; + const char *txt_land = "Land"; + const char *txt_pathplanner = "PathPlanner"; + const char *txt_poi = "PointOfInterest"; + const char *txt_autocruise = "AutoCruise"; + const char *txt_autotakeoff = "AutoTakeOff"; + const char *txt_autotune = "Autotune"; + const char *txt_disarmed = "Disarmed"; + const char *txt_arming = "Arming"; + const char *txt_armed = "Armed"; + + const char *txt_flightmode; + switch (telestate->FlightStatus.FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_MANUAL: + txt_flightmode = txt_manual; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: + txt_flightmode = txt_stabilized1; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: + txt_flightmode = txt_stabilized2; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: + txt_flightmode = txt_stabilized3; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4: + txt_flightmode = txt_stabilized4; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5: + txt_flightmode = txt_stabilized5; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6: + txt_flightmode = txt_stabilized6; + break; + case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: + txt_flightmode = txt_positionhold; + break; + case FLIGHTSTATUS_FLIGHTMODE_COURSELOCK: + txt_flightmode = txt_courselock; + break; + case FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM: + txt_flightmode = txt_velocityroam; + break; + case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH: + txt_flightmode = txt_homeleash; + break; + case FLIGHTSTATUS_FLIGHTMODE_ABSOLUTEPOSITION: + txt_flightmode = txt_absoluteposition; + break; + case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: + txt_flightmode = txt_returntobase; + break; + case FLIGHTSTATUS_FLIGHTMODE_LAND: + txt_flightmode = txt_land; + break; + case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: + txt_flightmode = txt_pathplanner; + break; + case FLIGHTSTATUS_FLIGHTMODE_POI: + txt_flightmode = txt_poi; + break; + case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE: + txt_flightmode = txt_autocruise; + break; + case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF: + txt_flightmode = txt_autotakeoff; + break; + case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE: + txt_flightmode = txt_autotune; + break; + default: + txt_flightmode = txt_unknown; + } + + const char *txt_armstate; + switch (telestate->FlightStatus.Armed) { + case FLIGHTSTATUS_ARMED_DISARMED: + txt_armstate = txt_disarmed; + break; + case FLIGHTSTATUS_ARMED_ARMING: + txt_armstate = txt_arming; + break; + case FLIGHTSTATUS_ARMED_ARMED: + txt_armstate = txt_armed; + break; + default: + txt_armstate = txt_unknown; + } + + snprintf(telestate->statusline, sizeof(telestate->statusline), "%12s,%8s", txt_flightmode, txt_armstate); +} + +/** + * generate warning beeps or spoken announcements + */ +uint8_t generate_warning() +{ + // set warning tone with hardcoded priority + if ((telestate->Settings.Warning.MinSpeed == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.MinSpeed > telestate->GPS.Groundspeed * MS_TO_KMH)) { + return HOTT_TONE_A; // maximum speed + } + if ((telestate->Settings.Warning.NegDifference2 == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.NegDifference2 > telestate->climbrate3s)) { + return HOTT_TONE_B; // sink rate 3s + } + if ((telestate->Settings.Warning.NegDifference1 == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.NegDifference1 > telestate->climbrate1s)) { + return HOTT_TONE_C; // sink rate 1s + } + if ((telestate->Settings.Warning.MaxDistance == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.MaxDistance < telestate->homedistance)) { + return HOTT_TONE_D; // maximum distance + } + if ((telestate->Settings.Warning.MinSensor1Temp == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.MinSensor1Temp > telestate->Gyro.temperature)) { + return HOTT_TONE_F; // minimum temperature sensor 1 + } + if ((telestate->Settings.Warning.MinSensor2Temp == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.MinSensor2Temp > telestate->Baro.Temperature)) { + return HOTT_TONE_G; // minimum temperature sensor 2 + } + if ((telestate->Settings.Warning.MaxSensor1Temp == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.MaxSensor1Temp < telestate->Gyro.temperature)) { + return HOTT_TONE_H; // maximum temperature sensor 1 + } + if ((telestate->Settings.Warning.MaxSensor2Temp == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.MaxSensor2Temp < telestate->Baro.Temperature)) { + return HOTT_TONE_I; // maximum temperature sensor 2 + } + if ((telestate->Settings.Warning.MaxSpeed == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.MaxSpeed < telestate->GPS.Groundspeed * MS_TO_KMH)) { + return HOTT_TONE_L; // maximum speed + } + if ((telestate->Settings.Warning.PosDifference2 == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.PosDifference2 > telestate->climbrate3s)) { + return HOTT_TONE_M; // climb rate 3s + } + if ((telestate->Settings.Warning.PosDifference1 == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.PosDifference1 > telestate->climbrate1s)) { + return HOTT_TONE_N; // climb rate 1s + } + if ((telestate->Settings.Warning.MinHeight == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.MinHeight > telestate->altitude)) { + return HOTT_TONE_O; // minimum height + } + if ((telestate->Settings.Warning.MinPowerVoltage == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.MinPowerVoltage > telestate->Battery.Voltage)) { + return HOTT_TONE_P; // minimum input voltage + } + if ((telestate->Settings.Warning.MaxUsedCapacity == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.MaxUsedCapacity < telestate->Battery.ConsumedEnergy)) { + return HOTT_TONE_V; // capacity + } + if ((telestate->Settings.Warning.MaxCurrent == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.MaxCurrent < telestate->Battery.Current)) { + return HOTT_TONE_W; // maximum current + } + if ((telestate->Settings.Warning.MaxPowerVoltage == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.MaxPowerVoltage < telestate->Battery.Voltage)) { + return HOTT_TONE_X; // maximum input voltage + } + if ((telestate->Settings.Warning.MaxHeight == HOTTBRIDGESETTINGS_WARNING_ENABLED) && + (telestate->Settings.Limit.MaxHeight < telestate->altitude)) { + return HOTT_TONE_Z; // maximum height + } + // altitude beeps when crossing altitude limits at 20,40,60,80,100,200,400,600,800 and 1000 meters + if (telestate->Settings.Warning.AltitudeBeep == HOTTBRIDGESETTINGS_WARNING_ENABLED) { + // update altitude when checked for beeps + float last = telestate->altitude_last; + float actual = telestate->altitude; + telestate->altitude_last = telestate->altitude; + if (((last < 20) && (actual >= 20)) || ((last > 20) && (actual <= 20))) { + return HOTT_TONE_20M; + } + if (((last < 40) && (actual >= 40)) || ((last > 40) && (actual <= 40))) { + return HOTT_TONE_40M; + } + if (((last < 60) && (actual >= 60)) || ((last > 60) && (actual <= 60))) { + return HOTT_TONE_60M; + } + if (((last < 80) && (actual >= 80)) || ((last > 80) && (actual <= 80))) { + return HOTT_TONE_80M; + } + if (((last < 100) && (actual >= 100)) || ((last > 100) && (actual <= 100))) { + return HOTT_TONE_100M; + } + if (((last < 200) && (actual >= 200)) || ((last > 200) && (actual <= 200))) { + return HOTT_TONE_200M; + } + if (((last < 400) && (actual >= 400)) || ((last > 400) && (actual <= 400))) { + return HOTT_TONE_400M; + } + if (((last < 600) && (actual >= 600)) || ((last > 600) && (actual <= 600))) { + return HOTT_TONE_600M; + } + if (((last < 800) && (actual >= 800)) || ((last > 800) && (actual <= 800))) { + return HOTT_TONE_800M; + } + if (((last < 1000) && (actual >= 1000)) || ((last > 1000) && (actual <= 1000))) { + return HOTT_TONE_1000M; + } + } + + // there is no warning + return 0; +} + +/** + * calculate checksum of data buffer + */ +uint8_t calc_checksum(uint8_t *data, uint16_t size) +{ + uint16_t sum = 0; + + for (int i = 0; i < size; i++) { + sum += data[i]; + } + return sum; +} + +/** + * scale float value with scale and offset to unsigned byte + */ +uint8_t scale_float2uint8(float value, float scale, float offset) +{ + uint16_t temp = (uint16_t)roundf(value * scale + offset); + uint8_t result; + + result = (uint8_t)temp & 0xff; + return result; +} + +/** + * scale float value with scale and offset to signed byte (int8_t) + */ +int8_t scale_float2int8(float value, float scale, float offset) +{ + int8_t result = (int8_t)roundf(value * scale + offset); + + return result; +} + +/** + * scale float value with scale and offset to word + */ +uword_t scale_float2uword(float value, float scale, float offset) +{ + uint16_t temp = (uint16_t)roundf(value * scale + offset); + uword_t result; + + result.l = (uint8_t)temp & 0xff; + result.h = (uint8_t)(temp >> 8) & 0xff; + return result; +} + +/** + * convert dword gps value into HoTT gps format and write result to given pointers + */ +void convert_long2gps(int32_t value, uint8_t *dir, uword_t *min, uword_t *sec) +{ + // convert gps decigrad value into degrees, minutes and seconds + uword_t temp; + uint32_t absvalue = abs(value); + uint16_t degrees = (absvalue / 10000000); + uint32_t seconds = (absvalue - degrees * 10000000) * 6; + uint16_t minutes = seconds / 1000000; + + seconds %= 1000000; + seconds = seconds / 100; + uint16_t degmin = degrees * 100 + minutes; + // write results + *dir = (value < 0) ? 1 : 0; + temp.l = (uint8_t)degmin & 0xff; + temp.h = (uint8_t)(degmin >> 8) & 0xff; + *min = temp; + temp.l = (uint8_t)seconds & 0xff; + temp.h = (uint8_t)(seconds >> 8) & 0xff; + *sec = temp; +} + +#endif // PIOS_INCLUDE_HOTT_BRIDGE /** * @} * @} diff --git a/flight/pios/common/pios_com.c b/flight/pios/common/pios_com.c index d13a7ca89..a1bfde67a 100644 --- a/flight/pios/common/pios_com.c +++ b/flight/pios/common/pios_com.c @@ -282,6 +282,30 @@ int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud) return 0; } +/** + * Change the port type to halfduplex (shared rx/tx medium) + * \param[in] port COM port + * \param[in] halfduplex enabled + * \return -1 if port not available + * \return 0 on success + */ +int32_t PIOS_COM_SetHalfDuplex(uint32_t com_id, bool halfduplex) +{ + struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id; + + if (!PIOS_COM_validate(com_dev)) { + /* Undefined COM port for this board (see pios_board.c) */ + return -1; + } + + /* Invoke the driver function if it exists */ + if (com_dev->driver->set_halfduplex) { + com_dev->driver->set_halfduplex(com_dev->lower_id, halfduplex); + } + + return 0; +} + int32_t PIOS_COM_ChangeConfig(uint32_t com_id, enum PIOS_COM_Word_Length word_len, enum PIOS_COM_StopBits stop_bits, enum PIOS_COM_Parity parity, uint32_t baud_rate, enum PIOS_COM_Mode mode) { struct pios_com_dev *com_dev = (struct pios_com_dev *)com_id; diff --git a/flight/pios/inc/pios_com.h b/flight/pios/inc/pios_com.h index 9364e5942..f576b346f 100644 --- a/flight/pios/inc/pios_com.h +++ b/flight/pios/inc/pios_com.h @@ -72,6 +72,7 @@ enum PIOS_COM_Mode { struct pios_com_driver { void (*init)(uint32_t id); void (*set_baud)(uint32_t id, uint32_t baud); + void (*set_halfduplex)(uint32_t id, bool halfduplex); void (*set_config)(uint32_t usart_id, enum PIOS_COM_Word_Length word_len, enum PIOS_COM_StopBits stop_bits, enum PIOS_COM_Parity parity, uint32_t baud_rate, enum PIOS_COM_Mode mode); void (*set_ctrl_line)(uint32_t id, uint32_t mask, uint32_t state); void (*tx_start)(uint32_t id, uint16_t tx_bytes_avail); @@ -91,6 +92,7 @@ struct pios_com_driver { /* Public Functions */ extern int32_t PIOS_COM_Init(uint32_t *com_id, const struct pios_com_driver *driver, uint32_t lower_id, uint8_t *rx_buffer, uint16_t rx_buffer_len, uint8_t *tx_buffer, uint16_t tx_buffer_len); extern int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud); +extern int32_t PIOS_COM_SetHalfDuplex(uint32_t com_id, bool halfduplex); extern int32_t PIOS_COM_ChangeConfig(uint32_t com_id, enum PIOS_COM_Word_Length word_len, enum PIOS_COM_StopBits stop_bits, enum PIOS_COM_Parity parity, uint32_t baud_rate, enum PIOS_COM_Mode mode); extern int32_t PIOS_COM_SetCtrlLine(uint32_t com_id, uint32_t mask, uint32_t state); extern int32_t PIOS_COM_RegisterCtrlLineCallback(uint32_t usart_id, pios_com_callback_ctrl_line ctrl_line_cb, uint32_t context); diff --git a/flight/pios/stm32f4xx/pios_usart.c b/flight/pios/stm32f4xx/pios_usart.c index 01d790963..e594f55e0 100644 --- a/flight/pios/stm32f4xx/pios_usart.c +++ b/flight/pios/stm32f4xx/pios_usart.c @@ -41,6 +41,7 @@ /* Provide a COM driver */ static void PIOS_USART_ChangeBaud(uint32_t usart_id, uint32_t baud); +static void PIOS_USART_SetHalfDuplex(uint32_t usart_id, bool halfduplex); static void PIOS_USART_ChangeConfig(uint32_t usart_id, enum PIOS_COM_Word_Length word_len, enum PIOS_COM_StopBits stop_bits, enum PIOS_COM_Parity parity, uint32_t baud_rate, enum PIOS_COM_Mode mode); static void PIOS_USART_SetCtrlLine(uint32_t usart_id, uint32_t mask, uint32_t state); static void PIOS_USART_RegisterRxCallback(uint32_t usart_id, pios_com_callback rx_in_cb, uint32_t context); @@ -49,13 +50,14 @@ static void PIOS_USART_TxStart(uint32_t usart_id, uint16_t tx_bytes_avail); static void PIOS_USART_RxStart(uint32_t usart_id, uint16_t rx_bytes_avail); const struct pios_com_driver pios_usart_com_driver = { - .set_baud = PIOS_USART_ChangeBaud, - .set_config = PIOS_USART_ChangeConfig, - .set_ctrl_line = PIOS_USART_SetCtrlLine, - .tx_start = PIOS_USART_TxStart, - .rx_start = PIOS_USART_RxStart, - .bind_tx_cb = PIOS_USART_RegisterTxCallback, - .bind_rx_cb = PIOS_USART_RegisterRxCallback, + .set_baud = PIOS_USART_ChangeBaud, + .set_halfduplex = PIOS_USART_SetHalfDuplex, + .set_config = PIOS_USART_ChangeConfig, + .set_ctrl_line = PIOS_USART_SetCtrlLine, + .tx_start = PIOS_USART_TxStart, + .rx_start = PIOS_USART_RxStart, + .bind_tx_cb = PIOS_USART_RegisterTxCallback, + .bind_rx_cb = PIOS_USART_RegisterRxCallback, }; enum pios_usart_dev_magic { @@ -286,6 +288,22 @@ static void PIOS_USART_ChangeBaud(uint32_t usart_id, uint32_t baud) USART_Init(usart_dev->cfg->regs, &usart_dev->init); } +/** + * Sets the USART peripheral into half duplex mode + * \param[in] usart_id USART name (GPS, TELEM, AUX) + * \param[in] bool wether to set half duplex or not + */ +static void PIOS_USART_SetHalfDuplex(uint32_t usart_id, bool halfduplex) +{ + struct pios_usart_dev *usart_dev = (struct pios_usart_dev *)usart_id; + + bool valid = PIOS_USART_validate(usart_dev); + + PIOS_Assert(valid); + + USART_HalfDuplexCmd(usart_dev->cfg->regs, halfduplex ? ENABLE : DISABLE); +} + /** * Changes configuration of the USART peripheral without re-initialising. * \param[in] usart_id USART name (GPS, TELEM, AUX) diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index f6d8d7987..84195dee9 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -126,6 +126,7 @@ UAVOBJSRCFILENAMES += poilearnsettings UAVOBJSRCFILENAMES += mpugyroaccelsettings UAVOBJSRCFILENAMES += txpidsettings UAVOBJSRCFILENAMES += txpidstatus +UAVOBJSRCFILENAMES += hottbridgesettings UAVOBJSRCFILENAMES += hottbridgestatus UAVOBJSRCFILENAMES += takeofflocation UAVOBJSRCFILENAMES += perfcounter diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index efe32a4be..c50d8c598 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -119,6 +119,7 @@ #define PIOS_INCLUDE_LED #define PIOS_INCLUDE_IAP #define PIOS_INCLUDE_SERVO +#define PIOS_INCLUDE_HOTT_BRIDGE /* #define PIOS_INCLUDE_I2C_ESC */ /* #define PIOS_INCLUDE_OVERO */ /* #define PIOS_OVERO_SPI */ diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 3470af273..5fd3343e7 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -1048,6 +1048,7 @@ void PIOS_Board_Init(void) case HWSETTINGS_RM_RCVRPORT_PPMMSP: case HWSETTINGS_RM_RCVRPORT_PPMMAVLINK: case HWSETTINGS_RM_RCVRPORT_PPMGPS: + case HWSETTINGS_RM_RCVRPORT_PPMHOTT: #if defined(PIOS_INCLUDE_PPM) PIOS_Board_configure_ppm(&pios_ppm_cfg); @@ -1097,6 +1098,10 @@ void PIOS_Board_Init(void) case HWSETTINGS_RM_RCVRPORT_PPMGPS: PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); break; + case HWSETTINGS_RM_RCVRPORT_HOTT: + case HWSETTINGS_RM_RCVRPORT_PPMHOTT: + PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, PIOS_COM_HOTT_RX_BUF_LEN, PIOS_COM_HOTT_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hott_id); + break; case HWSETTINGS_RM_RCVRPORT_IBUS: #if defined(PIOS_INCLUDE_IBUS) PIOS_Board_configure_ibus(&pios_usart_ibus_rcvr_cfg); diff --git a/ground/gcs/src/plugins/uavobjects/uavobjects.pro b/ground/gcs/src/plugins/uavobjects/uavobjects.pro index 23bce4c0b..46236e484 100644 --- a/ground/gcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/gcs/src/plugins/uavobjects/uavobjects.pro @@ -94,6 +94,7 @@ UAVOBJS = \ $${UAVOBJ_XML_DIR}/gyrosensor.xml \ $${UAVOBJ_XML_DIR}/gyrostate.xml \ $${UAVOBJ_XML_DIR}/homelocation.xml \ + $${UAVOBJ_XML_DIR}/hottbridgesettings.xml \ $${UAVOBJ_XML_DIR}/hottbridgestatus.xml \ $${UAVOBJ_XML_DIR}/hwsettings.xml \ $${UAVOBJ_XML_DIR}/i2cstats.xml \ diff --git a/shared/uavobjectdefinition/hottbridgesettings.xml b/shared/uavobjectdefinition/hottbridgesettings.xml new file mode 100644 index 000000000..81964dbc5 --- /dev/null +++ b/shared/uavobjectdefinition/hottbridgesettings.xml @@ -0,0 +1,82 @@ + + + Settings for the @ref HoTT Telemetry Module + + + VARIO + GPS + EAM + GAM + ESC + + + + + AltitudeBeep + MinSpeed + MaxSpeed + NegDifference1 + PosDifference1 + NegDifference2 + PosDifference2 + MinHeight + MaxHeight + MaxDistance + MinPowerVoltage + MaxPowerVoltage + MinSensor1Voltage + MaxSensor1Voltage + MinSensor2Voltage + MaxSensor2Voltage + MinCellVoltage + MaxCurrent + MaxUsedCapacity + MinSensor1Temp + MaxSensor1Temp + MinSensor2Temp + MaxSensor2Temp + MaxServoTemp + MinRPM + MaxRPM + MinFuel + MaxServoDifference + + + + + MinSpeed + MaxSpeed + NegDifference1 + PosDifference1 + NegDifference2 + PosDifference2 + MinHeight + MaxHeight + MaxDistance + MinPowerVoltage + MaxPowerVoltage + MinSensor1Voltage + MaxSensor1Voltage + MinSensor2Voltage + MaxSensor2Voltage + MinCellVoltage + MaxCurrent + MaxUsedCapacity + MinSensor1Temp + MaxSensor1Temp + MinSensor2Temp + MaxSensor2Temp + MaxServoTemp + MinRPM + MaxRPM + MinFuel + MaxServoDifference + + + + + + + + + diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index f0dfe1950..a8ecde363 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -11,9 +11,9 @@ - + limits="%0905NE:PPM+PWM:PPM+Telemetry:PPM+DebugConsole:PPM+ComBridge:PPM+MSP:PPM+MAVLink:PPM+GPS:PPM+HoTT:Telemetry:DebugConsole:ComBridge:MSP:MAVLink:GPS:HoTT:IBus;"/> diff --git a/shared/uavobjectdefinition/taskinfo.xml b/shared/uavobjectdefinition/taskinfo.xml index 1d7a2f4f1..57a98354f 100644 --- a/shared/uavobjectdefinition/taskinfo.xml +++ b/shared/uavobjectdefinition/taskinfo.xml @@ -31,6 +31,7 @@ GPS OSDGen + UAVOHoTTBridge UAVOMSPBridge AutoTune UAVOMAVLinkBridge @@ -66,6 +67,7 @@ GPS OSDGen + UAVOHoTTBridge UAVOMSPBridge AutoTune UAVOMAVLinkBridge @@ -105,6 +107,7 @@ GPS OSDGen + UAVOHoTTBridge UAVOMSPBridge AutoTune UAVOMAVLinkBridge From c5ea323770277659087000d6d33e5a1949532b53 Mon Sep 17 00:00:00 2001 From: Eric Price Date: Tue, 28 Mar 2017 07:58:50 +0200 Subject: [PATCH 6/7] LP-500 fixed comment typos found in PR/review --- flight/modules/UAVOHottBridge/uavohottbridge.c | 2 +- shared/uavobjectdefinition/hottbridgestatus.xml | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/flight/modules/UAVOHottBridge/uavohottbridge.c b/flight/modules/UAVOHottBridge/uavohottbridge.c index 56a442e36..9342e0eab 100644 --- a/flight/modules/UAVOHottBridge/uavohottbridge.c +++ b/flight/modules/UAVOHottBridge/uavohottbridge.c @@ -5,7 +5,7 @@ * @addtogroup UAVOHoTTBridge UAVO to HoTT Bridge Telemetry Module * @{ * - * @file uavohottridge.c + * @file uavohottbridge.c * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017. * Tau Labs, http://taulabs.org, Copyright (C) 2013-2014 * @brief sends telemery data on HoTT request diff --git a/shared/uavobjectdefinition/hottbridgestatus.xml b/shared/uavobjectdefinition/hottbridgestatus.xml index 06fe13a45..89e724999 100644 --- a/shared/uavobjectdefinition/hottbridgestatus.xml +++ b/shared/uavobjectdefinition/hottbridgestatus.xml @@ -1,7 +1,6 @@ HoTTBridge Status Information - From cae5668e647939d149dcdcffeb2bce6666da10db Mon Sep 17 00:00:00 2001 From: Eric Price Date: Tue, 28 Mar 2017 08:25:34 +0200 Subject: [PATCH 7/7] LP-500 Allow HoTT Telemetry on all USARTS (Rcvr, Flexi and Main) (trivial) --- .../targets/boards/revolution/firmware/pios_board.c | 12 +++++++++--- shared/uavobjectdefinition/hwsettings.xml | 8 ++++---- 2 files changed, 13 insertions(+), 7 deletions(-) diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 5fd3343e7..e5294dc3e 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -632,6 +632,9 @@ void PIOS_Board_Init(void) } #endif /* PIOS_INCLUDE_HOTT */ break; + case HWSETTINGS_RM_FLEXIPORT_HOTTTELEMETRY: + PIOS_Board_configure_com(&pios_usart_hott_flexi_cfg, PIOS_COM_HOTT_RX_BUF_LEN, PIOS_COM_HOTT_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hott_id); + break; case HWSETTINGS_RM_FLEXIPORT_EXBUS: #if defined(PIOS_INCLUDE_EXBUS) @@ -844,6 +847,9 @@ void PIOS_Board_Init(void) } #endif /* PIOS_INCLUDE_DEBUG_CONSOLE */ break; + case HWSETTINGS_RM_MAINPORT_HOTTTELEMETRY: + PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_HOTT_RX_BUF_LEN, PIOS_COM_HOTT_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hott_id); + break; case HWSETTINGS_RM_MAINPORT_COMBRIDGE: PIOS_Board_configure_com(&pios_usart_main_cfg, PIOS_COM_BRIDGE_RX_BUF_LEN, PIOS_COM_BRIDGE_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_bridge_id); break; @@ -1048,7 +1054,7 @@ void PIOS_Board_Init(void) case HWSETTINGS_RM_RCVRPORT_PPMMSP: case HWSETTINGS_RM_RCVRPORT_PPMMAVLINK: case HWSETTINGS_RM_RCVRPORT_PPMGPS: - case HWSETTINGS_RM_RCVRPORT_PPMHOTT: + case HWSETTINGS_RM_RCVRPORT_PPMHOTTTELEMETRY: #if defined(PIOS_INCLUDE_PPM) PIOS_Board_configure_ppm(&pios_ppm_cfg); @@ -1098,8 +1104,8 @@ void PIOS_Board_Init(void) case HWSETTINGS_RM_RCVRPORT_PPMGPS: PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, PIOS_COM_GPS_RX_BUF_LEN, PIOS_COM_GPS_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_gps_id); break; - case HWSETTINGS_RM_RCVRPORT_HOTT: - case HWSETTINGS_RM_RCVRPORT_PPMHOTT: + case HWSETTINGS_RM_RCVRPORT_HOTTTELEMETRY: + case HWSETTINGS_RM_RCVRPORT_PPMHOTTTELEMETRY: PIOS_Board_configure_com(&pios_usart_rcvrport_cfg, PIOS_COM_HOTT_RX_BUF_LEN, PIOS_COM_HOTT_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hott_id); break; case HWSETTINGS_RM_RCVRPORT_IBUS: diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index a8ecde363..fed486c90 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -11,11 +11,11 @@ - - - + limits="%0905NE:PPM+PWM:PPM+Telemetry:PPM+DebugConsole:PPM+ComBridge:PPM+MSP:PPM+MAVLink:PPM+GPS:PPM+HoTT Telemetry:Telemetry:DebugConsole:ComBridge:MSP:MAVLink:GPS:HoTT Telemetry:IBus;"/> + +