dRonin  adbada4
dRonin firmware
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Groups Pages
UAVOLighttelemetryBridge.c
Go to the documentation of this file.
1 
35 /*
36  * This program is free software; you can redistribute it and/or modify
37  * it under the terms of the GNU General Public License as published by
38  * the Free Software Foundation; either version 3 of the License, or
39  * (at your option) any later version.
40  *
41  * This program is distributed in the hope that it will be useful, but
42  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
43  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
44  * for more details.
45  *
46  * You should have received a copy of the GNU General Public License along
47  * with this program; if not, see <http://www.gnu.org/licenses/>
48  *
49  * Additional note on redistribution: The copyright and license notices above
50  * must be maintained in each individual source file that is a derivative work
51  * of this source file; otherwise redistribution is prohibited.
52  */
53 
54 #include "openpilot.h"
55 #include "modulesettings.h"
56 
57 #include "accels.h"
58 #include "airspeedactual.h"
59 #include "attitudeactual.h"
60 #include "baroaltitude.h"
61 #include "flightstatus.h"
62 #include "gpsposition.h"
63 #include "flightbatterysettings.h"
64 #include "flightbatterystate.h"
65 #include "manualcontrolcommand.h"
66 #include "positionactual.h"
67 
68 #include "pios_thread.h"
69 #include "pios_modules.h"
70 
71 #include <pios_hal.h>
72 
73 #if defined(PIOS_INCLUDE_LIGHTTELEMETRY)
74 // Private constants
75 #define STACK_SIZE_BYTES 600
76 #define TASK_PRIORITY PIOS_THREAD_PRIO_LOW
77 
78 #define CHUNK_TIME 30 /* 30ms. 3.6 bytes @ 1200, 14.4 bytes at 4800 */
79 /* The expected behavior at 1200 bps becomes then, prepare+send frame, delay 30ms,
80  * then prepare 1-3 frames at 30ms intervals that go unsent because of buffer.
81  * Then the next one goes.
82  */
83 
84 #define LTM_GFRAME_SIZE 18
85 #define LTM_AFRAME_SIZE 10
86 #define LTM_SFRAME_SIZE 11
87 
88 // Private types
89 
90 // Private variables
91 static bool module_enabled = false;
92 static struct pios_thread *taskHandle;
93 static uint32_t lighttelemetryPort;
94 static uint8_t ltm_scheduler;
95 static uint8_t ltm_slowrate;
96 
97 // Private functions
98 static void uavoLighttelemetryBridgeTask(void *parameters);
99 static void updateSettings();
100 
101 static int send_LTM_Packet(uint8_t *LTPacket, uint8_t LTPacket_size);
102 static int send_LTM_Gframe();
103 static int send_LTM_Aframe();
104 static int send_LTM_Sframe();
105 
106 
111 int32_t uavoLighttelemetryBridgeInitialize()
112 {
113  lighttelemetryPort = PIOS_COM_LIGHTTELEMETRY;
114 
115  if (lighttelemetryPort && PIOS_Modules_IsEnabled(PIOS_MODULE_UAVOLIGHTTELEMETRYBRIDGE)) {
116  // Update telemetry settings
117  module_enabled = true;
118  return 0;
119  }
120 
121  return -1;
122 }
123 
128 int32_t uavoLighttelemetryBridgeStart()
129 {
130  if ( module_enabled )
131  {
132  taskHandle = PIOS_Thread_Create(uavoLighttelemetryBridgeTask, "uavoLighttelemetryBridge", STACK_SIZE_BYTES, NULL, TASK_PRIORITY);
133  TaskMonitorAdd(TASKINFO_RUNNING_UAVOLIGHTTELEMETRYBRIDGE, taskHandle);
134  return 0;
135  }
136 
137  return -1;
138 }
139 
140 MODULE_INITCALL(uavoLighttelemetryBridgeInitialize, uavoLighttelemetryBridgeStart);
141 
142 
143 /*#######################################################################
144  * Module thread, should not return.
145  *#######################################################################
146 */
147 
148 static void uavoLighttelemetryBridgeTask(void *parameters)
149 {
150  updateSettings();
151 
152  // Main task loop
153  while (1)
154  {
155  int ret = 0;
156 
157  if (!ret) {
158  switch (ltm_scheduler) {
159  case 0:
160  case 6:
161  ret = send_LTM_Sframe();
162  break;
163 
164  case 3:
165  case 9:
166  ret = send_LTM_Gframe();
167  break;
168 
169  case 1:
170  case 4:
171  case 7:
172  case 10:
173  if (ltm_slowrate) {
174  break;
175  }
176 
177  case 2:
178  case 5:
179  case 8:
180  case 11:
181  ret = send_LTM_Aframe();
182  break;
183 
184  default:
185  break;
186  }
187  }
188 
189  PIOS_Thread_Sleep(CHUNK_TIME);
190 
191  if (ret) {
192  /* If we couldn't tx, go around and try the same thing
193  * again.
194  */
195  continue;
196  }
197 
198  ltm_scheduler++;
199 
200  if (ltm_scheduler > 11) {
201  ltm_scheduler = 0;
202  }
203  }
204 }
205 
206 /*#######################################################################
207  * Internal functions
208  *#######################################################################
209 */
210 //GPS packet
211 static int send_LTM_Gframe()
212 {
213  GPSPositionData pdata = { }; /* Set to all 0's */
214 
215  bool have_gps = false;
216 
217  if (GPSPositionHandle() != NULL) {
218  have_gps = true;
219  GPSPositionGet(&pdata);
220  }
221 
222  int32_t lt_latitude = pdata.Latitude;
223  int32_t lt_longitude = pdata.Longitude;
224  uint8_t lt_groundspeed = (uint8_t)roundf(pdata.Groundspeed); //rounded m/s .
225  int32_t lt_altitude = 0;
226  if (PositionActualHandle() != NULL) {
227  float altitude;
228  PositionActualDownGet(&altitude);
229  lt_altitude = (int32_t)roundf(altitude * -100.0f);
230  } else if (BaroAltitudeHandle() != NULL) {
231  float altitude;
232  BaroAltitudeAltitudeGet(&altitude);
233  lt_altitude = (int32_t)roundf(altitude * 100.0f); //Baro alt in cm.
234  } else if (have_gps) {
235  lt_altitude = (int32_t)roundf(pdata.Altitude * 100.0f); //GPS alt in cm.
236  } else {
237  return 0; /* Don't even bother, no data for this frame! */
238  }
239 
240  uint8_t lt_gpsfix;
241  switch (pdata.Status) {
242  case GPSPOSITION_STATUS_NOGPS:
243  lt_gpsfix = 0;
244  break;
245  case GPSPOSITION_STATUS_NOFIX:
246  lt_gpsfix = 1;
247  break;
248  case GPSPOSITION_STATUS_FIX2D:
249  lt_gpsfix = 2;
250  break;
251  case GPSPOSITION_STATUS_FIX3D:
252  lt_gpsfix = 3;
253  break;
254  default:
255  lt_gpsfix = 0;
256  break;
257  }
258 
259  uint8_t lt_gpssats = (int8_t)pdata.Satellites;
260  //pack G frame
261  uint8_t LTBuff[LTM_GFRAME_SIZE];
262  //G Frame: $T(2 bytes)G(1byte)LAT(cm,4 bytes)LON(cm,4bytes)SPEED(m/s,1bytes)ALT(cm,4bytes)SATS(6bits)FIX(2bits)CRC(xor,1byte)
263  //START
264  LTBuff[0] = 0x24; //$
265  LTBuff[1] = 0x54; //T
266  //FRAMEID
267  LTBuff[2] = 0x47; //G
268  //PAYLOAD
269  LTBuff[3] = (lt_latitude >> 8*0) & 0xFF;
270  LTBuff[4] = (lt_latitude >> 8*1) & 0xFF;
271  LTBuff[5] = (lt_latitude >> 8*2) & 0xFF;
272  LTBuff[6] = (lt_latitude >> 8*3) & 0xFF;
273  LTBuff[7] = (lt_longitude >> 8*0) & 0xFF;
274  LTBuff[8] = (lt_longitude >> 8*1) & 0xFF;
275  LTBuff[9] = (lt_longitude >> 8*2) & 0xFF;
276  LTBuff[10] = (lt_longitude >> 8*3) & 0xFF;
277  LTBuff[11] = (lt_groundspeed >> 8*0) & 0xFF;
278  LTBuff[12] = (lt_altitude >> 8*0) & 0xFF;
279  LTBuff[13] = (lt_altitude >> 8*1) & 0xFF;
280  LTBuff[14] = (lt_altitude >> 8*2) & 0xFF;
281  LTBuff[15] = (lt_altitude >> 8*3) & 0xFF;
282  LTBuff[16] = ((lt_gpssats << 2)& 0xFF ) | (lt_gpsfix & 0b00000011) ; // last 6 bits: sats number, first 2:fix type (0,1,2,3)
283 
284  return send_LTM_Packet(LTBuff,LTM_GFRAME_SIZE);
285 }
286 
287 //Attitude packet
288 static int send_LTM_Aframe()
289 {
290  //prepare data
291  AttitudeActualData adata;
292  AttitudeActualGet(&adata);
293  int16_t lt_pitch = (int16_t)(roundf(adata.Pitch)); //-180/180°
294  int16_t lt_roll = (int16_t)(roundf(adata.Roll)); //-180/180°
295  int16_t lt_heading = (int16_t)(roundf(adata.Yaw)); //-180/180°
296  //pack A frame
297  uint8_t LTBuff[LTM_AFRAME_SIZE];
298 
299  //A Frame: $T(2 bytes)A(1byte)PITCH(2 bytes)ROLL(2bytes)HEADING(2bytes)CRC(xor,1byte)
300  //START
301  LTBuff[0] = 0x24; //$
302  LTBuff[1] = 0x54; //T
303  //FRAMEID
304  LTBuff[2] = 0x41; //A
305  //PAYLOAD
306  LTBuff[3] = (lt_pitch >> 8*0) & 0xFF;
307  LTBuff[4] = (lt_pitch >> 8*1) & 0xFF;
308  LTBuff[5] = (lt_roll >> 8*0) & 0xFF;
309  LTBuff[6] = (lt_roll >> 8*1) & 0xFF;
310  LTBuff[7] = (lt_heading >> 8*0) & 0xFF;
311  LTBuff[8] = (lt_heading >> 8*1) & 0xFF;
312 
313  return send_LTM_Packet(LTBuff,LTM_AFRAME_SIZE);
314 }
315 
316 //Sensors packet
317 static int send_LTM_Sframe()
318 {
319  //prepare data
320  uint16_t lt_vbat = 0;
321  uint16_t lt_amp = 0;
322  uint8_t lt_rssi = 0;
323  uint8_t lt_airspeed = 0;
324  uint8_t lt_arm = 0;
325  uint8_t lt_failsafe = 0;
326  uint8_t lt_flightmode = 0;
327 
328 
329  if (FlightBatteryStateHandle() != NULL) {
330  FlightBatteryStateData sdata;
331  FlightBatteryStateGet(&sdata);
332  lt_vbat = (uint16_t)roundf(sdata.Voltage*1000); //Battery voltage in mv
333  lt_amp = (uint16_t)roundf(sdata.ConsumedEnergy); //mA consumed
334  }
335  if (ManualControlCommandHandle() != NULL) {
336  ManualControlCommandData mdata;
337  ManualControlCommandGet(&mdata);
338  lt_rssi = (uint8_t)mdata.Rssi; //RSSI in %
339  }
340  if (AirspeedActualHandle() != NULL) {
341  AirspeedActualData adata;
342  AirspeedActualGet (&adata);
343  lt_airspeed = (uint8_t)roundf(adata.TrueAirspeed); //Airspeed in m/s
344  } else if (GPSPositionHandle() != NULL) {
345  float groundspeed;
346  GPSPositionGroundspeedGet(&groundspeed);
347  lt_airspeed = (uint8_t)roundf(groundspeed);
348  }
349 
350  FlightStatusData fdata;
351  FlightStatusGet(&fdata);
352  lt_arm = fdata.Armed; //Armed status
353  if (lt_arm == 1) //arming , we don't use this one
354  lt_arm = 0;
355  else if (lt_arm == 2) // armed
356  lt_arm = 1;
357  if (fdata.ControlSource == FLIGHTSTATUS_CONTROLSOURCE_FAILSAFE)
358  lt_failsafe = 1;
359  else
360  lt_failsafe = 0;
361 
362  // Flight mode(0-19): 0: Manual, 1: Rate, 2: Attitude/Angle, 3: Horizon, 4: Acro, 5: Stabilized1, 6: Stabilized2, 7: Stabilized3,
363  // 8: Altitude Hold, 9: Loiter/GPS Hold, 10: Auto/Waypoints, 11: Heading Hold / headFree,
364  // 12: Circle, 13: RTH, 14: FollowMe, 15: LAND, 16:FlybyWireA, 17: FlybywireB, 18: Cruise, 19: Unknown
365 
366  switch (fdata.FlightMode) {
367  case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
368  lt_flightmode = 0; break;
369  case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
370  lt_flightmode = 5; break;
371  case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
372  lt_flightmode = 6; break;
373  case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
374  lt_flightmode = 7; break;
375  case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
376  lt_flightmode = 8; break;
377  case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
378  lt_flightmode = 9; break;
379  case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME:
380  lt_flightmode = 13; break;
381  case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
382  lt_flightmode = 10; break;
383  default:
384  lt_flightmode = 19; //Unknown
385  }
386  //pack A frame
387  uint8_t LTBuff[LTM_SFRAME_SIZE];
388 
389  //A Frame: $T(2 bytes)A(1byte)PITCH(2 bytes)ROLL(2bytes)HEADING(2bytes)CRC(xor,1byte)
390  //START
391  LTBuff[0] = 0x24; //$
392  LTBuff[1] = 0x54; //T
393  //FRAMEID
394  LTBuff[2] = 0x53; //S
395  //PAYLOAD
396  LTBuff[3] = (lt_vbat >> 8*0) & 0xFF;
397  LTBuff[4] = (lt_vbat >> 8*1) & 0xFF;
398  LTBuff[5] = (lt_amp >> 8*0) & 0xFF;
399  LTBuff[6] = (lt_amp >> 8*1) & 0xFF;
400  LTBuff[7] = (lt_rssi >> 8*0) & 0xFF;
401  LTBuff[8] = (lt_airspeed >> 8*0) & 0xFF;
402  LTBuff[9] = ((lt_flightmode << 2)& 0xFF ) | ((lt_failsafe << 1)& 0b00000010 ) | (lt_arm & 0b00000001) ; // last 6 bits: flight mode, 2nd bit: failsafe, 1st bit: Arm status.
403 
404  return send_LTM_Packet(LTBuff,LTM_SFRAME_SIZE);
405 }
406 
407 static int send_LTM_Packet(uint8_t *LTPacket, uint8_t LTPacket_size)
408 {
409  //calculate Checksum
410  uint8_t LTCrc = 0x00;
411  for (int i = 3; i < LTPacket_size-1; i++) {
412  LTCrc ^= LTPacket[i];
413  }
414  LTPacket[LTPacket_size-1] = LTCrc;
415 
416  int ret = PIOS_COM_SendBufferNonBlocking(lighttelemetryPort,
417  LTPacket, LTPacket_size);
418 
419  if (ret < 0) {
420  return -1;
421  }
422 
423  return 0;
424 }
425 
426 static void updateSettings()
427 {
428  // Retrieve settings
429  uint8_t speed;
430  ModuleSettingsLightTelemetrySpeedGet(&speed);
431 
432  PIOS_HAL_ConfigureSerialSpeed(lighttelemetryPort, speed);
433 
434  if (speed == MODULESETTINGS_LIGHTTELEMETRYSPEED_1200)
435  ltm_slowrate = 1;
436  else
437  ltm_slowrate = 0;
438 }
439 #endif //end define lighttelemetry
440 
speed
uint16_t speed
Definition: msp_messages.h:101
updateSettings
static void updateSettings()
Definition: ComUsbBridge.c:171
PIOS_COM_SendBufferNonBlocking
int32_t PIOS_COM_SendBufferNonBlocking(uintptr_t com_id, const uint8_t *buffer, uint16_t len)
taskHandle
static struct pios_thread * taskHandle
Definition: actuator.c:83
pios_hal.h
PIOS_Modules_IsEnabled
bool PIOS_Modules_IsEnabled(enum pios_modules module)
Definition: pios_modules.c:41
STACK_SIZE_BYTES
#define STACK_SIZE_BYTES
Definition: actuator.c:62
module_enabled
bool module_enabled
Definition: charonscreendisplay.c:79
TASK_PRIORITY
#define TASK_PRIORITY
Definition: actuator.c:65
altitude
uint16_t altitude
Definition: msp_messages.h:100
PIOS_HAL_ConfigureSerialSpeed
void PIOS_HAL_ConfigureSerialSpeed(uintptr_t com_id, HwSharedSpeedBpsOptions speed)
Definition: pios_hal.c:1115
PIOS_COM_LIGHTTELEMETRY
#define PIOS_COM_LIGHTTELEMETRY
Definition: pios_board.h:120
PIOS_MODULE_UAVOLIGHTTELEMETRYBRIDGE
Definition: pios_modules.h:40
MODULE_INITCALL
#define MODULE_INITCALL(ifn, sfn)
Definition: pios_initcall.h:67
PIOS_Thread_Create
struct pios_thread * PIOS_Thread_Create(void(*fp)(void *), const char *namep, size_t stack_bytes, void *argp, enum pios_thread_prio_e prio)
Definition: pios_thread.c:89
groundspeed
uint16_t groundspeed
Definition: pios_crossfire.h:43
TaskMonitorAdd
int32_t TaskMonitorAdd(TaskInfoRunningElem task, struct pios_thread *threadp)
Definition: taskmonitor.c:67
i
uint8_t i
Definition: msp_messages.h:97
pios_modules.h
PIOS_Thread_Sleep
void PIOS_Thread_Sleep(uint32_t time_ms)
Definition: pios_thread.c:229
px_mkfw.f
tuple f
Definition: px_mkfw.py:81
openpilot.h
Includes PiOS and core architecture components.
pios_thread.h