mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-26 15:54:15 +01:00
LP-500 HoTT bridge module initial create
This commit is contained in:
Corvus Corax
Eric Price
parent
a14469e68b
commit
2977ec1382
589
flight/modules/UAVOHottBridge/uavohottbridge.c
Normal file
589
flight/modules/UAVOHottBridge/uavohottbridge.c
Normal file
@ -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
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
Loading…
x
Reference in New Issue
Block a user