Paparazzi UAS  v5.18.0_stable-1-g6993852-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
max7456.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013-2020 Chris Efstathiou hendrixgr@gmail.com
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
28 #include "std.h"
29 //#include "stdio.h"
30 
31 #include "inter_mcu.h"
32 
33 #include "mcu_periph/sys_time.h"
34 #include "mcu_periph/gpio.h"
35 #include "mcu_periph/spi.h"
36 
37 #include "generated/flight_plan.h"
38 #include "generated/airframe.h"
39 #include "autopilot.h"
40 #include "subsystems/electrical.h"
41 #include "state.h"
42 
43 // for GetPosAlt, include correct header until we have unified API
44 #if AP
45 //#include "subsystems/navigation/nav.h"
47 #else
49 #endif
50 #if DOWNLINK
52 #endif
53 
54 // Peripherials
57 
58 #define OSD_STRING_SIZE 31
59 #define osd_sprintf _osd_sprintf
60 
61 #if !defined(OSD_USE_FLOAT_LOW_PASS_FILTERING)
62 #define OSD_USE_FLOAT_LOW_PASS_FILTERING
63 #endif
64 
65 //LOW PASS filter strength, cannot be 0, MAX=16
66 #if !defined(AMPS_LOW_PASS_FILTER_STRENGTH) || AMPS_LOW_PASS_FILTER_STRENGTH == 0
67 #define AMPS_LOW_PASS_FILTER_STRENGTH 6
68 #endif
69 
70 #if !defined(SPEED_LOW_PASS_FILTER_STRENGTH) || SPEED_LOW_PASS_FILTER_STRENGTH == 0
71 #define SPEED_LOW_PASS_FILTER_STRENGTH 6
72 #endif
73 
74 #if !defined(BAT_CAPACITY)
75 #pragma message "BAT_CAPACITY not defined, 5000 mah will be used."
76 #define BAT_CAPACITY 5000.0
77 #endif
78 
79 #if AP
80 
81 #if !defined(LOITER_BAT_CURRENT)
82 #pragma message "LOITER_BAT_CURRENT not defined, 10 Amps will be used for LOITER current draw."
83 #define LOITER_BAT_CURRENT 10.0
84 #endif
85 
86 #if !defined(STALL_AIRSPEED)
87 #pragma message "STALL_AIRSPEED not defined, 10 m/s will be used."
88 #define STALL_AIRSPEED 10.0
89 #endif
90 
91 #if !defined(MINIMUM_AIRSPEED)
92 #pragma message "MINIMUM_AIRSPEED not defined, 1.3 * STALL_SPEED will be used"
93 #define MINIMUM_AIRSPEED (1.3f * STALL_AIRSPEED)
94 #endif
95 
96 #endif
97 
98 #if !defined(IMU_MAG_X_SIGN)
99 #define IMU_MAG_X_SIGN 1
100 #endif
101 #if !defined(IMU_MAG_X_SIGN)
102 #define IMU_MAG_Y_SIGN 1
103 #endif
104 #if !defined(IMU_MAG_X_SIGN)
105 #define IMU_MAG_Z_SIGN 1
106 #endif
107 
108 
109 typedef struct {
110  float fx;
111  float fy;
112  float fz;
113 } VECTOR;
114 
115 typedef struct {
116  float fx1; float fx2; float fx3;
117  float fy1; float fy2; float fy3;
118  float fz1; float fz2; float fz3;
119 } MATRIX;
120 
121 #if AP
122 static void mag_compass(void);
123 #endif
124 static void send_mag_heading(struct transport_tx *trans, struct link_device *dev);
125 static void vSubtractVectors(VECTOR *svA, VECTOR svB, VECTOR svC);
126 static void vMultiplyMatrixByVector(VECTOR *svA, MATRIX smB, VECTOR svC);
127 static float home_direction(void);
128 static char ascii_to_osd_c(char c);
129 static void calc_flight_time_left(void);
130 static void draw_osd(void);
131 static void osd_put_s(char *string, uint8_t attributes, uint8_t char_nb, uint8_t row, uint8_t column);
132 static bool _osd_sprintf(char *buffer, char *string, float value);
133 
135 
140 char osd_char = ' ';
144 
157 };
158 
162  L_JUST = 0x00,
163  R_JUST = 0x01,
164  C_JUST = 0x02,
165 
166 };
167 
173 
174 float mag_course_deg = 0;
175 float mag_heading_rad = 0;
176 float gps_course_deg = 0;
177 float home_dir_deg = 0;
178 
179 #if AP
180 // Periodic function called with a frequency defined in the module .xml file
181 void mag_compass(void)
182 {
183 
184  struct FloatEulers *att = stateGetNedToBodyEulers_f();
185  struct Int32Vect3 mag;
186  struct Int32Vect3 mag_neutrals;
187  int32_t x = 0, y = 0, z = 0;
188  float cos_roll; float sin_roll; float cos_pitch; float sin_pitch; float mag_x; float mag_y;
189  static float mag_declination = 0;
190  static bool declination_calculated = false;
191 
193  VECT3_COPY(mag_neutrals, imu.mag_neutral);
194 #if (defined(IMU_MAG_X_SENS) && defined(IMU_MAG_Y_SENS) && defined(IMU_MAG_Z_SENS)) && !defined(USE_MAGNETOMETER)
195  x = ((mag.x - mag_neutrals.x) * IMU_MAG_X_SIGN * IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN;
196  y = ((mag.y - mag_neutrals.y) * IMU_MAG_Y_SIGN * IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN;
197  z = ((mag.z - mag_neutrals.z) * IMU_MAG_Z_SIGN * IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN;
198 #else
199  x = (mag.x - mag_neutrals.x) * IMU_MAG_X_SIGN;
200  y = (mag.y - mag_neutrals.y) * IMU_MAG_Y_SIGN;
201  z = (mag.z - mag_neutrals.z) * IMU_MAG_Z_SIGN;
202 #endif
203 
204  cos_roll = cosf(att->phi);
205  sin_roll = sinf(att->phi);
206  cos_pitch = cosf(att->theta);
207  sin_pitch = sinf(att->theta);
208  // Pitch&Roll Compensation:
209  mag_x = x * cos_pitch + y * sin_roll * sin_pitch + z * cos_roll * sin_pitch;
210  mag_y = y * cos_roll - z * sin_roll;
211 
212  // Magnetic Heading N = 0, E = 90, S = +-180, W = -90
213  mag_heading_rad = atan2(-mag_y, mag_x);
214 #if defined(AHRS_MAG_DECLINATION)
215  if (AHRS_MAG_DECLINATION != 0.0) {
216  //conversion from degrees to radians is done in the airframe.h file
217  mag_heading_rad = mag_heading_rad + AHRS_MAG_DECLINATION;
218  }
219 #endif
220  if (mag_heading_rad > M_PI) { // Angle normalization (-180 deg to 180 deg)
221  mag_heading_rad -= (2.0 * M_PI);
222  } else if (mag_heading_rad < -M_PI) { mag_heading_rad += (2.0 * M_PI); }
223 
224  if (declination_calculated == false) {
225 #if defined(NOMINAL_AIRSPEED)
226  if (gps.fix == GPS_FIX_3D && stateGetHorizontalSpeedNorm_f() > (float)NOMINAL_AIRSPEED) {
227 #else
228  if (gps.fix == GPS_FIX_3D && stateGetHorizontalSpeedNorm_f() > 10.0) {
229 #endif
230  mag_declination = stateGetHorizontalSpeedDir_f();
231  if (mag_declination > M_PI) { // Angle normalization (-180 deg to 180 deg)
232  mag_declination -= (2.0 * M_PI);
233  } else if (mag_declination < -M_PI) { mag_declination += (2.0 * M_PI); }
234  mag_declination -= mag_heading_rad;
235  declination_calculated = true;
236  if (fabs(mag_declination) > RadOfDeg(10.)) { mag_declination = 0; declination_calculated = false; }
237  }
238  }
239  mag_heading_rad = mag_heading_rad + mag_declination;
240  if (mag_heading_rad > M_PI) { // Angle normalization (-180 deg to 180 deg)
241  mag_heading_rad -= (2.0 * M_PI);
242  } else if (mag_heading_rad < -M_PI) { mag_heading_rad += (2.0 * M_PI); }
243  // Magnetic COMPASS Heading N = 0, E = 90, S = 180, W = 270
244  mag_course_deg = DegOfRad(mag_heading_rad);
245  if (mag_course_deg < 0) { mag_course_deg += 360; }
246 
247  return;
248 }
249 #endif
250 
251 
252 static void send_mag_heading(struct transport_tx *trans, struct link_device *dev)
253 {
254 
255 #if DOWNLINK
256  pprz_msg_send_IMU_MAG(trans, dev, AC_ID, &mag_course_deg, &gps_course_deg, &home_dir_deg);
257 #endif
258 
259  return;
260 }
261 
262 //*******************************************************************
263 // function name: vSubtractVectors
264 // description: subtracts two vectors a = b - c
265 // parameters:
266 //*******************************************************************
267 static void vSubtractVectors(VECTOR *svA, VECTOR svB, VECTOR svC)
268 {
269  svA->fx = svB.fx - svC.fx;
270  svA->fy = svB.fy - svC.fy;
271  svA->fz = svB.fz - svC.fz;
272 }
273 
274 //*******************************************************************
275 // function name: vMultiplyMatrixByVector
276 // description: multiplies matrix by vector svA = smB * svC
277 // parameters:
278 //*******************************************************************
279 static void vMultiplyMatrixByVector(VECTOR *svA, MATRIX smB, VECTOR svC)
280 {
281  svA->fx = smB.fx1 * svC.fx + smB.fx2 * svC.fy + smB.fx3 * svC.fz;
282  svA->fy = smB.fy1 * svC.fx + smB.fy2 * svC.fy + smB.fy3 * svC.fz;
283  svA->fz = smB.fz1 * svC.fx + smB.fz2 * svC.fy + smB.fz3 * svC.fz;
284 }
285 
286 
287 static float home_direction(void)
288 {
289 
290  static VECTOR svPlanePosition, Home_Position, Home_PositionForPlane, Home_PositionForPlane2;
291  static MATRIX smRotation;
292  float home_dir = 0;
293 
294 
295  /*
296  By swapping coordinates (fx=fPlaneNorth, fy=fPlaneEast) we make the the circle angle go from 0 (0 is to the top of the circle)
297  to 360 degrees or from 0 radians to 2 PI radians in a clockwise rotation. This way the GPS reported angle can be directly
298  applied to the rotation matrices (in radians).
299  In standard mathematical notation 0 is to the right (East) of the circle, -90 is to the bottom, +-180 is to the left
300  and +90 is to the top (counterclockwise rotation).
301  When reading back the actual rotated coordinates fx has the y coordinate and fy has the x when
302  represented on a circle in standard mathematical notation.
303  */
304  if (gps.fix == GPS_FIX_3D && stateGetHorizontalSpeedNorm_f() > 5.0) { //Only when flying
305  svPlanePosition.fx = stateGetPositionEnu_f()->y;
306  svPlanePosition.fy = stateGetPositionEnu_f()->x;
307  svPlanePosition.fz = stateGetPositionUtm_f()->alt;
308 #ifdef AP
309  Home_Position.fx = WaypointY(WP_HOME);
310  Home_Position.fy = WaypointX(WP_HOME);
311  Home_Position.fz = GetAltRef();
312 #else
313  Home_Position.fx = waypoint_get_x(WP_HOME);
314  Home_Position.fy = waypoint_get_y(WP_HOME);
315  Home_Position.fz = 0;
316 #endif
317 
318  /* distance between plane and object */
319  vSubtractVectors(&Home_PositionForPlane, Home_Position, svPlanePosition);
320 
321  /* yaw */
322  smRotation.fx1 = cosf(stateGetHorizontalSpeedDir_f());
323  smRotation.fx2 = sinf(stateGetHorizontalSpeedDir_f());
324  smRotation.fx3 = 0.;
325  smRotation.fy1 = -smRotation.fx2;
326  smRotation.fy2 = smRotation.fx1;
327  smRotation.fy3 = 0.;
328  smRotation.fz1 = 0.;
329  smRotation.fz2 = 0.;
330  smRotation.fz3 = 1.;
331 
332  vMultiplyMatrixByVector(&Home_PositionForPlane2, smRotation, Home_PositionForPlane);
333 
334  /* DEFAULT ORIENTATION IS 0 = FRONT, 90 = RIGHT, 180 = BACK, -90 = LEFT
335  *
336  * WHEN home_dir = (float)(atan2(Home_PositionForPlane2.fy, (Home_PositionForPlane2.fx)));
337  *
338  * plane front
339  * 0˚
340  * ^
341  * I
342  * -45˚ I 45˚
343  * \ I /
344  * \I/
345  * -90˚-------I------- 90˚
346  * /I\
347  * / I \
348  * -135˚ I 135˚
349  * I
350  * 180
351  * plane back
352  *
353  *
354  * When the home_dir variable goes to 0 the aircraft is headed straight back home
355  */
356 
357  /* fixed to the plane*/
358  home_dir = atan2(Home_PositionForPlane2.fy, Home_PositionForPlane2.fx);
359  if (home_dir > M_PI) { // Angle normalization (-180 deg to 180 deg but still in radians)
360  home_dir -= (2.0 * M_PI);
361  } else if (home_dir < -M_PI) { home_dir += (2.0 * M_PI); }
362  home_dir_deg = DegOfRad(home_dir); // Now convert radians to degrees.
363 
364  } // END OF if (gps.fix == GPS_FIX_3D) statement.
365 
366  return (home_dir_deg);
367 }
368 
369 static char ascii_to_osd_c(char c)
370 {
371 
372 #if defined USE_MATEK_TYPE_OSD_CHIP && USE_MATEK_TYPE_OSD_CHIP == 1
373  PRINT_CONFIG_MSG("OSD USES THE CUSTOM MATEK TYPE OSD CHIP")
374 
375  return (c);
376 
377 #else
378  PRINT_CONFIG_MSG("OSD USES THE STANDARF MAX7456 OSD CHIP")
379 
380  if (c >= '0' && c <= '9') {
381  if (c == '0') { c -= 38; } else { c -= 48; }
382  } else if (c >= 'A' && c <= 'Z') {
383  c -= 54;
384  } else if (c >= 'a' && c <= 'z') {
385  c -= 60;
386 
387  } else {
388  switch (c) {
389  case ('('): c = 0x3f; break;
390  case (')'): c = 0x40; break;
391  case ('.'): c = 0x41; break;
392  case ('?'): c = 0x42; break;
393  case (';'): c = 0x43; break;
394  case (':'): c = 0x44; break;
395  case (','): c = 0x45; break;
396  //case('''): c = 0x46; break;
397  case ('/'): c = 0x47; break;
398  case ('"'): c = 0x48; break;
399  case ('-'): c = 0x49; break;
400  case ('<'): c = 0x4A; break;
401  case ('>'): c = 0x4B; break;
402  case ('@'): c = 0x4C; break;
403  case (' '): c = 0x00; break;
404  case ('\0'): c = 0xFF; break;
405  default : break;
406  }
407  }
408 
409  return (c);
410 
411 #endif
412 }
413 
414 static void calc_flight_time_left(void)
415 {
416  float current_amps = 0;
417  float horizontal_speed = 0;
418  float bat_capacity_left = 0;
419  static float bat_capacity_used = 0;
420 
421 
422  current_amps = electrical.current;
423  bat_capacity_used += (current_amps * 1000.) / (3600. * (float)MAX7456_PERIODIC_FREQ);
424  bat_capacity_left = (float)BAT_CAPACITY - bat_capacity_used;
425  if (bat_capacity_left < 0) { bat_capacity_left = 0; }
426  horizontal_speed = stateGetHorizontalSpeedNorm_f();
427 
428 #if AP
429  if (stateGetHorizontalSpeedNorm_f() < 5.0 || autopilot.launch == false) {
430  current_amps = LOITER_BAT_CURRENT;
431  horizontal_speed = MINIMUM_AIRSPEED;
432  }
433 #else // #if AP
434  current_amps = 1.0; // FIXME, Find how to tell if the rotorcraft is on the ground or it is flying.
435  horizontal_speed = 10.0;
436 #endif
437 
438 #if defined(OSD_USE_FLOAT_LOW_PASS_FILTERING)
439 
440  static double current_amps_sum = 0;
441  static double horizontal_speed_sum = 0;
442  static float current_amps_filtered = 0;
443  static float horizontal_speed_filtered = 0;
444 
445  current_amps_sum = (current_amps_sum - current_amps_filtered) + current_amps;
446  current_amps_filtered = current_amps_sum / (1 << AMPS_LOW_PASS_FILTER_STRENGTH);
447  current_amps = current_amps_filtered;
448 
449  horizontal_speed_sum = (horizontal_speed_sum - horizontal_speed_filtered) + horizontal_speed;
450  horizontal_speed_filtered = horizontal_speed_sum / (1 << SPEED_LOW_PASS_FILTER_STRENGTH);
451  horizontal_speed = horizontal_speed_filtered;
452 
453 #else
454 
455  uint32_t current_amps_int = 0;
456  uint32_t horizontal_speed_int = 0;
457  static uint64_t current_amps_sum = 0;
458  static uint64_t horizontal_speed_sum = 0;
459  static uint32_t current_amps_filtered = 0;
460  static uint32_t horizontal_speed_filtered = 0;
461 
462  // LOW PASS FILTERS for making the OSD 'max_flyable_distance_left' var change more gently.
463  current_amps_int = (uint32_t)(current_amps * 1000.0);
464  horizontal_speed_int = (uint32_t)(horizontal_speed * 1000.0);
465 
466  current_amps_sum = (current_amps_sum - current_amps_filtered) + current_amps_int;
467  current_amps_filtered = (current_amps_sum + (1 << (AMPS_LOW_PASS_FILTER_STRENGTH - 1))) >>
469 
470  horizontal_speed_sum = (horizontal_speed_sum - horizontal_speed_filtered) + horizontal_speed_int;
471  horizontal_speed_filtered = (horizontal_speed_sum + (1 << (SPEED_LOW_PASS_FILTER_STRENGTH - 1))) >>
473 
474  current_amps = (float)(current_amps_filtered) / 1000.;
475  horizontal_speed = (float)(horizontal_speed_filtered) / 1000.;
476 
477 #endif
478 
479  max_flyable_distance_left = ((bat_capacity_left / (current_amps * 1000)) * 3600) * horizontal_speed;
480 
481  return;
482 }
483 
484 
485 static void osd_put_s(char *string, uint8_t attributes, uint8_t char_nb, uint8_t row, uint8_t column)
486 {
487 
488  int8_t x = 0, idx = 0, post_offset = 0, aft_offset = 0, string_len = 0;
489  char osd_buf[OSD_STRING_SIZE];
490 
491  if (row > 15) { column = 15; }
492  if (column > 29) { column = 29; }
493 
494 // translate the string and put it to the "osd_string" '\0' = 0xff
495  x = 0;
496  while (*(string + x) != '\0') { osd_string[x] = ascii_to_osd_c(*(string + x)); x++; }
497  osd_string[x] = 0xff;
498  string_len = x;
499  idx = x;
500 
501  if (attributes & C_JUST) {
502  if (char_nb % 2 == 0) { char_nb++; }
503  post_offset = (char_nb - string_len) / 2;
504  aft_offset = char_nb - string_len;
505  if (((int8_t)column - (char_nb / 2)) >= 0) { column -= (char_nb / 2); }
506  for (x = 0; x < 30; x++) { osd_buf[x] = 0; } // FILL WITH SPACES
507  // COPY THE ORIGINAL STRING TO ITS NEW POSITION
508  for (x = 0; x < string_len; x++) { osd_buf[post_offset + x] = osd_string[x]; }
509  osd_buf[string_len + aft_offset] = 0xFF; // TERMINATE THE MODIFIED STRING
510  // COPY THE MODIFIED STRING TO MAIN OSD STRING
511  x = 0;
512  do { osd_string[x] = osd_buf[x]; } while (osd_buf[x++] != 0xFF);
513  } else if (attributes & R_JUST) {
514  //if(x){ x -= 1; }
515  //if (char_nb < string_len){ char_nb = string_len; }
516  if (((int8_t)column - char_nb) >= 0) { column -= char_nb; }
517  if (((int8_t)char_nb - string_len) >= 0) { post_offset = char_nb - string_len; } else {post_offset = 0; }
518  //ADD LEADING SPACES
519  //First shift right the string and then add spaces at the beggining
520  while (idx >= 0) { osd_string[idx + post_offset] = osd_string[idx]; idx--; }
521  idx = 0;
522  while (idx < post_offset) { osd_string[idx] = 0; idx++; }
523  //osd_string[idx] = 0xff;
524 
525  } else {
526  //Adjust for the reserved character number.
527  for (x = 0; x < (int8_t)(sizeof(osd_string)); x++) { if (osd_string[x] == 0xFF) { break; } }
528  for (; x < char_nb; x++) { osd_string[x] = 0; }
529  osd_string[x] = 0xff;
530  }
531 
532  osd_char_address = ((uint16_t)row * 30) + column;
533  osd_attr = (attributes & (BLINK | INVERT));
534 //TRIGGER THE SPI TRANSFERS. The rest of the spi transfers occur in the "max7456_event" function.
535  if (max7456_osd_status == OSD_IDLE) {
538  max7456_trans.output_buf[1] = (uint8_t)((osd_char_address >> 8) & 0x0001);
540  spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
541 
542  }
543 
544  return;
545 }
546 
547 
548 static bool _osd_sprintf(char *buffer, char *string, float value)
549 {
550  uint8_t param_start = 0;
551  uint8_t param_end = 0;
552  uint8_t frac_nb = 0;
553  uint8_t digit = 0;
554  uint8_t x = 0, y = 0, z = 0;
555 
556  uint16_t i_dec = 0;
557  uint16_t i_frac = 0;
558 
559  char to_asc[10] = {48, 48, 48, 48, 48, 48, 48, 48, 48, 48};
560  char string_buf[OSD_STRING_SIZE];
561 
562 // Clear the osd string.
563  for (x = 0; x < sizeof(osd_string); x++) { osd_string[x] = 0; }
564  for (x = 0; x < sizeof(string_buf); x++) { string_buf[x] = 0; }
565 
566 //copy the string passed as parameter to a buffer
567  for (x = 0; x < sizeof(string_buf); x++) { string_buf[x] = *(string + x); if (string_buf[x] == '\0') { break; } }
568  do {
569  x = 0;
570  param_start = 0;
571  param_end = 0;
572  //Now check for any special character
573  while (string_buf[x] != '\0') {
574  // EXAMPLE: in "%160c"x is '%' x+4 = 'c' and x+1='1', x+2='6' and x+3='0'
575  if (string_buf[x] == '%') { if (string_buf[x + 4] == 'c') { (param_start = x + 1); param_end = x + 3; break; } }
576  x++;
577  }
578  if (param_end - param_start) {
579  //load the special character value where the % character was
580  string_buf[x] = ((string_buf[param_start] - 48) * 100) + ((string_buf[param_start + 1] - 48) * 10) +
581  (string_buf[param_start + 2] - 48);
582  x++; // increment x to the next character which should be the first special character's digit
583  //Move the rest of the buffer forward so only the special character remains,
584  // for example in %170c '%' now has the special character's code and x now points to '1'
585  // which will be overwritten with the rest of the string after the 'c'
586  for (y = (x + 4); y <= sizeof(string_buf); y++) { string_buf[x++] = string_buf[y]; }
587  }
588  } while ((param_end - param_start > 0));
589 
590 // RESET THE USED VARIABLES JUST TO BE SAFE.
591  x = 0;
592  y = 0;
593  param_start = 0;
594  param_end = 0;
595 // Search for the prameter start and stop positions.
596  while (string_buf[x] != '\0') {
597  if (string_buf[x] == '%') {
598  param_start = x;
599 
600  } else if (string_buf[x] == 'f') { param_end = x; break; }
601  x++;
602  }
603  if (param_end - param_start) {
604  // find and bound the precision specified.
605  frac_nb = string_buf[param_end - 1] - 48; // Convert to number, ASCII 48 = '0'
606  if (frac_nb > 3) { frac_nb = 3; } // Bound value.
607 
608  y = (sizeof(to_asc) - 1); // Point y to the end of the array.
609  i_dec = abs((int16_t)value);
610  // Fist we will deal with the fractional part if specified.
611  if (frac_nb > 0 && frac_nb <= 3) {
612  i_frac = abs((int16_t)((value - (int16_t)value) * 1000)); // Max precision is 3 digits.
613  x = 100;
614  z = frac_nb;
615  do { // Example if frac_nb=2 then 952 will show as .95
616  z--;
617  digit = (i_frac / x);
618  to_asc[y + z] = digit + 48; // Convert to ASCII
619  i_frac -= digit * x; // Calculate the remainder.
620  x /= 10; // 952-(9*100) = 52, 52-(10*5)=2 etc.
621 
622  } while (z > 0);
623 
624  y -= frac_nb; // set y to point where the dot must be placed.
625  to_asc[y] = '.';
626  y--; // Set y to point where the rest of the numbers must be written.
627 
628  } // if (frac_nb > 0 && frac_nb <= 3){
629 
630  // Now it is time for the integer part. "y" already points to the position just before the dot.
631  do {
632  to_asc[y] = (i_dec % 10) + 48; //Write at least one digit even if value is zero.
633  i_dec /= 10;
634  if (i_dec <= 0) { // This way the leading zero is ommited.
635  if (value < 0) { y--; to_asc[y] = '-'; } // Place the minus sign if needed.
636  break;
637 
638  } else { y--; }
639 
640  } while (1);
641 
642  // Fill the buffer with the characters in the beggining of the string if any.
643  for (x = 0; x < param_start; x++) { *(buffer + x) = string_buf[x]; }
644 
645  // x is now pointing to the next character in osd_string.
646  // y is already pointing to the first digit or negative sign in "to_asc" array.
647  while (y < sizeof(to_asc)) { *(buffer + x) = to_asc[y]; x++; y++; }
648  // x is now pointing to the next character in osd_string.
649  // "param_end" is pointing to the last format character in the string.
650  do {
651  param_end++;
652  *(buffer + x++) = string_buf[param_end];
653 
654  } while (string_buf[param_end] != '\0'); //Write the rest of the string including the terminating char.
655 
656  // End of if (param_end - param_start)
657  } else {
658  for (x = 0; x < sizeof(string_buf); x++) {
659  *(buffer + x) = string_buf[x]; //Write the rest of the string including the terminating char.
660  if (*(buffer + x) == '\0') { break; }
661  }
662  }
663 
664  return (0);
665 }
666 
667 void draw_osd(void)
668 {
669  float temp = 0;
670  float altitude = 0;
671  float distance_to_home = 0;
672  static float home_direction_degrees = 0;
673 #if defined(BARO_ALTITUDE_VAR)
674  static float baro_alt_correction = 0;
675 #endif
676 
677  struct FloatEulers *att = stateGetNedToBodyEulers_f();
678  struct EnuCoor_f *pos = stateGetPositionEnu_f();
679 #if AP
680  float ph_x = waypoints[WP_HOME].x - pos->x;
681  float ph_y = waypoints[WP_HOME].y - pos->y;
682  float stall_speed = STALL_AIRSPEED;
683 #else // FOR ROTORCRAFTS
684  float ph_x = waypoint_get_x(WP_HOME) - pos->x;
685  float ph_y = waypoint_get_y(WP_HOME) - pos->y;
686 #endif // #if AP
687 
688  distance_to_home = (float)(sqrt(ph_x * ph_x + ph_y * ph_y));
690 #if AP
691  mag_compass();
692 #endif
693 
694  //THE SWITCH STATEMENT ENSURES THAT ONLY ONE SPI TRANSACTION WILL OCUUR IN EVERY PERIODIC FUNCTION CALL
695  switch (step) {
696 
697  case (0):
698  if (gps.fix == GPS_FIX_3D && stateGetHorizontalSpeedNorm_f() > 10.0) { //Only when flying
699  gps_course_deg = (float)gps.course;
700  gps_course_deg = DegOfRad(gps_course_deg / 1e7);
701  } else {
702 #if AP
704 #else
706 #endif
707  }
709  osd_put_s(osd_string, C_JUST, 3, 1, 15);
710  step = 10;
711  break;
712 
713  case (10):
714  //Only when flying because i need this indication to remain stable if the GPS is lost.
715  // This way i can still have the synchronized magnetic compass heading and the last bearing home.
716  if (gps.fix == GPS_FIX_3D && gps.pdop < 1000 && stateGetHorizontalSpeedNorm_f() > 5.0) { //Only when flying
717  home_direction_degrees = gps_course_deg + home_direction(); //home_direction returns degrees -180 to +180
718  if (home_direction_degrees < 0) { home_direction_degrees += 360; } // translate the -180, +180 to 0-360.
719  if (home_direction_degrees >= 360) { home_direction_degrees -= 360; }
720  }
721 #if defined(USE_MATEK_TYPE_OSD_CHIP) && USE_MATEK_TYPE_OSD_CHIP == 1
722  // All special character codes must be in 3 digit format!
723  osd_sprintf(osd_string, "%191c%.0f", home_direction_degrees); // 0 when heading straight back home.
724  osd_put_s(osd_string, C_JUST, 5, 2, 15); // "false = L_JUST
725 #else
726  osd_sprintf(osd_string, "H%.0f", home_direction_degrees);
727  osd_put_s(osd_string, C_JUST, 5, 2, 15); // "false = L_JUST
728 
729 #endif
730  step = 20;
731  break;
732 
733  case (20):
734  temp = ((float)electrical.vsupply);
735  osd_sprintf(osd_string, "%.1fV", temp);
736  if (temp > LOW_BAT_LEVEL) {
737  osd_put_s(osd_string, L_JUST, 5, 1, 1);
738 
739  } else { osd_put_s(osd_string, (L_JUST | BLINK | INVERT), 5, 1, 1); }
740  step = 30;
741  break;
742 
743  case (30):
744  if (gps.fix == GPS_FIX_3D) {
745 #if defined(USE_MATEK_TYPE_OSD_CHIP) && USE_MATEK_TYPE_OSD_CHIP == 1
746  //Since we only send one special character the float variable is replaced by a zero.
747  osd_sprintf(osd_string, "%030c%031c", 0);
748  osd_put_s(osd_string, false, 2, 2, 1);
749 #else
750  osd_put_s("**", false, 2, 2, 1);
751 #endif
752  } else {
753 #if defined(USE_MATEK_TYPE_OSD_CHIP) && USE_MATEK_TYPE_OSD_CHIP == 1
754  //Since we only send one special character the float variable is replaced by a zero.
755  osd_sprintf(osd_string, "%030c%031c", 0); // ALL special osd chars must have 3 digits.
756  osd_put_s(osd_string, (L_JUST | BLINK), 2, 2, 1);
757 #else
758  osd_put_s("**", (L_JUST | BLINK), 2, 2, 1);
759 #endif
760  }
761  step = 40;
762  break;
763 
764  case (40):
765 #if AP
766  if (autopilot.mode == AP_MODE_AUTO2) {
767  osd_put_s("A2", L_JUST, 2, 2, 3);
768  } else if (autopilot.mode == AP_MODE_AUTO1) {
769  osd_put_s("A1", L_JUST, 2, 2, 3);
770  } else {
771  osd_put_s("MAN", L_JUST, 3, 2, 3);
772  }
773 #endif
774  step = 50;
775  break;
776 
777  case (50):
778 #if AP
779  osd_sprintf(osd_string, "THR%.0f", (((float)ap_state->commands[COMMAND_THROTTLE] / (float)MAX_PPRZ) * 100.));
780 #else
781  osd_sprintf(osd_string, "THR%.0fTHR", (((float)stabilization_cmd[COMMAND_THRUST] / (float)MAX_PPRZ) * 100.));
782 #endif
783  osd_put_s(osd_string, L_JUST, 6, 3, 1);
784  step = 60;
785  break;
786 
787  case (60):
788 #if AP
789 #if defined(USE_MATEK_TYPE_OSD_CHIP) && USE_MATEK_TYPE_OSD_CHIP == 1
790  if ((fabs(stateGetHorizontalSpeedNorm_f() * cos(att->phi))) < stall_speed) {
791  osd_sprintf(osd_string, "STALL!", 0);
792  osd_put_s(osd_string, (R_JUST | BLINK), 6, 1, 30);
793  } else {
794  osd_sprintf(osd_string, "%.0f%161c", (stateGetHorizontalSpeedNorm_f() * 3.6));
795  osd_put_s(osd_string, R_JUST, 6, 1, 30);
796  }
797 #else
798  if ((fabs(stateGetHorizontalSpeedNorm_f() * cos(att->phi))) < stall_speed) {
799  osd_sprintf(osd_string, "STALL!", 0);
800  osd_put_s(osd_string, (R_JUST | BLINK), 6, 1, 30);
801  } else {
803  osd_put_s(osd_string, R_JUST, 6, 1, 30);
804  }
805 #endif
806 
807 #else // #if AP
808 
809 #if defined(USE_MATEK_TYPE_OSD_CHIP) && USE_MATEK_TYPE_OSD_CHIP == 1
810  osd_sprintf(osd_string, "%.0f%161c", (stateGetHorizontalSpeedNorm_f() * 3.6));
811 #else
813 #endif
814  osd_put_s(osd_string, R_JUST, 6, 1, 30);
815 
816 #endif
817  step = 70;
818  break;
819 
820  case (70):
821 #if defined(BARO_ALTITUDE_VAR)
822  if (gps.fix == GPS_FIX_3D && gps.pdop < 1000) {
823  RunOnceEvery((MAX7456_PERIODIC_FREQ * 300), { baro_alt_correction = GetPosAlt() - BARO_ALTITUDE_VAR;});
824  altitude = GetPosAlt();
825  } else {
826  altitude = BARO_ALTITUDE_VAR + baro_alt_correction;
827  }
828 #else
829  altitude = GetPosAlt();
830 #endif
831 #if defined(USE_MATEK_TYPE_OSD_CHIP) && USE_MATEK_TYPE_OSD_CHIP == 1
832  osd_sprintf(osd_string, "%.0f%177c", altitude);
833 #else
834  osd_sprintf(osd_string, "%.0fM", altitude);
835 #endif
836  osd_put_s(osd_string, R_JUST, 6, 2, 30); // "false = L_JUST
837  step = 80;
838  break;
839 
840  case (80):
841 #if defined(USE_MATEK_TYPE_OSD_CHIP) && USE_MATEK_TYPE_OSD_CHIP == 1
842  osd_sprintf(osd_string, "%.1f%159c", stateGetSpeedEnu_f()->z);
843 #else
845 #endif
846  if (stateGetSpeedEnu_f()->z > 3.0) {
847  osd_put_s(osd_string, (R_JUST | BLINK), 6, 3, 30);
848  } else {
849  osd_put_s(osd_string, R_JUST, 6, 3, 30);
850  }
851  step = 90;
852  break;
853 
854  case (90):
855 #if defined(USE_MATEK_TYPE_OSD_CHIP) && USE_MATEK_TYPE_OSD_CHIP == 1
856  // ANY SPECIAL CHARACTER CODE MUST BE A 3 DIGIT NUMBER WITH THE LEADING ZEROS!!!!
857  // THE SPECIAL CHARACTER CAN BE PLACED BEFORE OR AFTER THE FLOAT OR ANY OTHER CHARACTER
858  osd_sprintf(osd_string, "%160c%.1fK%012c", (distance_to_home / 1000));
859  osd_put_s(osd_string, L_JUST, 7, 15, 1);
860 #else
861  osd_sprintf(osd_string, "%.1fKM", (distance_to_home / 1000));
862  osd_put_s(osd_string, L_JUST, 7, 15, 1);
863 #endif
864  step = 100;
865  break;
866 
867  case (100):
868 #if defined(USE_MATEK_TYPE_OSD_CHIP) && USE_MATEK_TYPE_OSD_CHIP == 1
869  osd_sprintf(osd_string, "%147c%.1fK%012c", (max_flyable_distance_left / 1000));
870 #else
871  osd_sprintf(osd_string, "%.1fKM", (max_flyable_distance_left / 1000));
872 #endif
873  if ((max_flyable_distance_left + 1000.0) < distance_to_home) {
874  osd_put_s(osd_string, (R_JUST | BLINK), 7, 15, 30);
875  } else {
876  osd_put_s(osd_string, (R_JUST), 7, 15, 30);
877  }
878  step = 110;
879  break;
880 
881  // A Text PFD as graphics are not the strong point of the MAX7456
882  // In order to level the aircraft while fpving
883  // just move the stick to the opposite direction from the angles shown on the osd
884  // and that's why positive pitch (UP) is shown below the OSD center
885  case (110):
886  if (DegOfRad(att->theta) > 3) {
887  osd_sprintf(osd_string, "%.0f", DegOfRad(att->theta));
888  osd_put_s(osd_string, C_JUST, 5, 6, 15);
889 
890  } else { osd_put_s(" ", C_JUST, 5, 6, 15); }
891  step = 112;
892  break;
893 
894  case (112):
895  if (DegOfRad(att->theta) < -3) {
896  osd_sprintf(osd_string, "%.0f", DegOfRad(att->theta));
897  osd_put_s(osd_string, C_JUST, 5, 10, 15);
898 
899  } else { osd_put_s(" ", C_JUST, 5, 10, 15); }
900  step = 114;
901  break;
902 
903  case (114):
904  if (DegOfRad(att->phi) > 3) {
905  osd_sprintf(osd_string, "%.0f>", DegOfRad(att->phi));
906  osd_put_s(osd_string, false, 5, 8, 18);
907 
908  } else { osd_put_s(" ", false, 5, 8, 18); }
909  step = 116;
910  break;
911 
912  case (116):
913  if (DegOfRad(att->phi) < -3) {
914  osd_sprintf(osd_string, "<%.0f", DegOfRad(fabs(att->phi)));
915  osd_put_s(osd_string, R_JUST, 5, 8, 13);
916 
917  } else { osd_put_s(" ", R_JUST, 5, 8, 13); }
918  step = 120;
919  break;
920 
921  case (120):
922 #if defined(USE_MATEK_TYPE_OSD_CHIP) && USE_MATEK_TYPE_OSD_CHIP == 1
923  osd_sprintf(osd_string, "%126c", 0);
924  osd_put_s(osd_string, false, 1, 8, 15); // false = L_JUST
925 #else
926  osd_put_s("+", false, 1, 8, 15); // false = L_JUST
927 #endif
928  step = 0;
929  break;
930 
931  default: step = 0; break;
932  } // End of switch statement.
933 
934  return;
935 }
936 
937 void max7456_init(void)
938 {
939 
940  max7456_trans.slave_idx = MAX7456_SLAVE_IDX;
951  max7456_trans.before_cb = NULL;
952  max7456_trans.after_cb = NULL;
953 
954  osd_enable = 1;
957 
958 #if DOWNLINK
960 #endif
961  home_direction();
962 
963  return;
964 }
965 
967 {
968 
969  //This code is executed always and checks if the "osd_enable" var has been changed by telemetry.
970  //If yes then it commands a reset but this time turns on or off the osd overlay, not the video.
971  if (max7456_osd_status == OSD_IDLE) {
972  if (osd_enable > 1) {
973  osd_enable = 1;
974  }
975  if ((osd_enable << 3) != osd_enable_val) {
976  osd_enable_val = (osd_enable << 3);
978  }
979  }
980 
981  //INITIALIZATION OF THE OSD
983  step = 0;
986  //This operation needs at least 100us but when the periodic function will be invoked again
987  //sufficient time will have elapsed even with at a periodic frequency of 1000 Hz
989  //We give an extra delay step by going to the event function and back here for the Reset to complete.
991  spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
992  } else if (max7456_osd_status == OSD_INIT2) {
997  spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
998  } else if (max7456_osd_status == OSD_IDLE && osd_enable > 0) {
999  draw_osd();
1000  } // End of if (max7456_osd_status == OSD_UNINIT)
1001 
1002 
1003 
1004  return;
1005 }
1006 
1007 void max7456_event(void)
1008 {
1009 
1010  static uint8_t x = 0;
1011 
1014 
1015  switch (max7456_osd_status) {
1016  case (OSD_INIT1):
1018  break;
1019  case (OSD_INIT3):
1023  //Max7456 requires that you read first and then change only bit4 of the OSDBL register.
1024  //Reading was started in the periodic function and now we rerwrite the OSDBL register.
1025  max7456_trans.output_buf[1] = max7456_trans.input_buf[1] & (~(1 << 4));
1027  spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
1028  break;
1029  case (OSD_INIT4):
1031 #if USE_PAL_FOR_OSD_VIDEO
1032 #pragma message "Camera and OSD must be both PAL or NTSC otherwise only the camera picture will be visible."
1034 #else
1036 #endif
1038  spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
1039  break;
1040  case (OSD_S_STEP1):
1045  spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
1046  break;
1047  case (OSD_S_STEP2):
1052  spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
1053  x = 0;
1054  break;
1055  case (OSD_S_STEP3):
1056  max7456_trans.output_length = 1; //1 byte tranfers, auto address incrementing.
1057  if (osd_string[x] != 0XFF) {
1059  spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
1060  } else {
1061  max7456_trans.output_buf[0] = 0xFF; //Exit the auto increment mode
1063  spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
1064  }
1065  break;
1066  case (OSD_FINISHED):
1071  spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
1072  break;
1073  case (OSD_READ_STATUS):
1080  spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
1081  } else {
1082  osd_attr = 0;
1085  }
1086  break;
1087 
1088  default: break;
1089  }
1090  }
1091  return;
1092 }
float mag_course_deg
Definition: max7456.c:174
max7456_osd_status_codes
Definition: max7456.c:145
unsigned short uint16_t
Definition: types.h:16
bool launch
request launch
Definition: autopilot.h:71
enum SPIClockPolarity cpol
clock polarity control
Definition: spi.h:155
char osd_char
Definition: max7456.c:140
#define AP_MODE_AUTO1
Communication between fbw and ap processes.
static uint32_t idx
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
float x
Definition: common_nav.h:40
char osd_str_buf[OSD_STRING_SIZE]
Definition: max7456.c:139
enum SPIClockDiv cdiv
prescaler of main clock to use as SPI clock
Definition: spi.h:159
#define BAT_CAPACITY
Definition: max7456.c:76
float mag_heading_rad
Definition: max7456.c:175
float phi
in radians
uint16_t output_length
number of data words to write
Definition: spi.h:152
static void vSubtractVectors(VECTOR *svA, VECTOR svB, VECTOR svC)
Definition: max7456.c:267
#define OSD_BLINK_CHAR
Definition: max7456_regs.h:53
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
#define IMU_MAG_Z_SENS_DEN
Definition: imu_ardrone2.h:79
Maxim MAX7456 single-channel monochrome on-screen display driver.
float fx2
Definition: point.c:91
Periodic telemetry system header (includes downlink utility and generated code).
vector in East North Up coordinates Units: meters
#define LOW_BAT_LEVEL
low battery level in Volts (for 3S LiPo)
Definition: electrical.h:36
Some architecture independent helper functions for GPIOs.
void max7456_init(void)
Definition: max7456.c:937
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
float fy1
Definition: point.c:92
float fz2
Definition: point.c:93
Maxim MAX7456 single-channel monochrome on-screen display driver.
SPI transaction structure.
Definition: spi.h:148
float psi
in radians
osd_attributes
Definition: max7456.c:159
float vsupply
supply voltage in V
Definition: electrical.h:45
bool spi_submit(struct spi_periph *p, struct spi_transaction *t)
Submit SPI transaction.
Definition: spi_arch.c:511
#define OSD_DMM_REG
Definition: max7456_regs.h:36
enum SPIBitOrder bitorder
MSB/LSB order.
Definition: spi.h:158
static void draw_osd(void)
Definition: max7456.c:667
#define OSD_DMAH_REG
Definition: max7456_regs.h:37
struct Imu imu
global IMU state
Definition: imu.c:108
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:39
float fy2
Definition: point.c:92
float home_dir_deg
Definition: max7456.c:177
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
#define OSD_IMAGE_ENABLE
Definition: max7456_regs.h:48
volatile uint8_t * output_buf
pointer to transmit buffer for DMA
Definition: spi.h:150
#define IMU_MAG_Z_SIGN
Definition: imu_hbmini.h:57
float fx3
Definition: point.c:91
enum SPISlaveSelect select
slave selection behavior
Definition: spi.h:154
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:50
static int32_t altitude
Definition: airspeed_uADC.c:59
#define OSD_RESET
Definition: max7456_regs.h:50
struct ap_state * ap_state
Definition: inter_mcu.c:37
#define osd_sprintf
Definition: max7456.c:59
euler angles
Architecture independent SPI (Serial Peripheral Interface) API.
#define OSD_AUTO_INCREMENT_MODE
Definition: max7456_regs.h:56
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
Definition: imu.h:48
void max7456_event(void)
Definition: max7456.c:1007
enum SPIClockPhase cpha
clock phase control
Definition: spi.h:156
float theta
in radians
#define IMU_MAG_Y_SENS_NUM
Definition: imu_ardrone2.h:75
uint8_t osd_stat_reg
Definition: max7456.c:171
#define OSD_OSDBL_REG_R
Definition: max7456_regs.h:41
#define IMU_MAG_X_SENS_NUM
Definition: imu_ardrone2.h:72
#define OSD_OSDBL_REG
Definition: max7456_regs.h:40
struct spi_transaction max7456_trans
Definition: max7456.c:134
static void calc_flight_time_left(void)
Definition: max7456.c:414
float y
Definition: common_nav.h:41
unsigned long long uint64_t
Definition: types.h:20
Definition: spi.h:90
float fy
Definition: point.c:86
#define AP_MODE_AUTO2
static bool _osd_sprintf(char *buffer, char *string, float value)
Definition: max7456.c:548
#define WaypointX(_wp)
Definition: common_nav.h:45
#define OSD_STAT_REG
Definition: max7456_regs.h:42
float x
in meters
Interface for electrical status: supply voltage, current, battery status, etc.
struct Int32Vect3 mag_neutral
magnetometer neutral readings (bias) in raw/unscaled units
Definition: imu.h:45
float fy3
Definition: point.c:92
Architecture independent timing functions.
#define SPEED_LOW_PASS_FILTER_STRENGTH
Definition: max7456.c:71
float waypoint_get_x(uint8_t wp_id)
Get X/East coordinate of waypoint in meters.
Definition: waypoints.c:77
float fz1
Definition: point.c:93
float fx1
Definition: point.c:91
float fx
Definition: point.c:85
SPICallback after_cb
NULL or function called after the transaction.
Definition: spi.h:161
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:105
#define STALL_AIRSPEED
Definition: energy_ctrl.c:151
unsigned long uint32_t
Definition: types.h:18
char osd_string[OSD_STRING_SIZE]
Definition: max7456.c:138
float waypoint_get_y(uint8_t wp_id)
Get Y/North coordinate of waypoint in meters.
Definition: waypoints.c:85
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
signed short int16_t
Definition: types.h:17
static float home_direction(void)
Definition: max7456.c:287
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
uint8_t osd_attr
Definition: max7456.c:143
float gps_course_deg
Definition: max7456.c:176
Definition: point.c:84
static void vMultiplyMatrixByVector(VECTOR *svA, MATRIX smB, VECTOR svC)
Definition: max7456.c:279
CPOL = 0.
Definition: spi.h:83
uint8_t step
Definition: max7456.c:141
float fz
Definition: point.c:87
#define WaypointY(_wp)
Definition: common_nav.h:46
uint32_t max_flyable_distance_left
Definition: max7456.c:172
uint16_t input_length
number of data words to read
Definition: spi.h:151
signed long int32_t
Definition: types.h:19
#define IMU_MAG_X_SENS_DEN
Definition: imu_ardrone2.h:73
#define OSD_DMAL_REG
Definition: max7456_regs.h:38
#define OSD_VM0_REG
Definition: max7456_regs.h:34
uint8_t max7456_osd_status
Definition: max7456.c:168
static void send_mag_heading(struct transport_tx *trans, struct link_device *dev)
Definition: max7456.c:252
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
Core autopilot interface common to all firmwares.
Rotorcraft navigation functions.
#define OSD_INVERT_PIXELS
Definition: max7456_regs.h:54
static void osd_put_s(char *string, uint8_t attributes, uint8_t char_nb, uint8_t row, uint8_t column)
Definition: max7456.c:485
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition: state.h:944
#define OSD_VIDEO_MODE_PAL
Definition: max7456_regs.h:45
Definition: point.c:90
#define IMU_MAG_Y_SIGN
Definition: imu_hbmini.h:56
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:99
void max7456_periodic(void)
Definition: max7456.c:966
static struct EnuCoor_f * stateGetSpeedEnu_f(void)
Get ground speed in local ENU coordinates (float).
Definition: state.h:917
CPHA = 0.
Definition: spi.h:74
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
#define AMPS_LOW_PASS_FILTER_STRENGTH
Definition: max7456.c:67
#define OSD_STRING_SIZE
Definition: max7456.c:58
float z
in meters
uint8_t osd_enable_val
Definition: max7456.c:170
slave is selected before transaction and unselected after
Definition: spi.h:63
#define IMU_MAG_Z_SENS_NUM
Definition: imu_ardrone2.h:78
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
enum SPIDataSizeSelect dss
data transfer word size
Definition: spi.h:157
uint8_t slave_idx
slave id: SPI_SLAVE0 to SPI_SLAVE4
Definition: spi.h:153
uint8_t osd_spi_tx_buffer[2]
Definition: max7456.c:136
Definition: spi.h:125
volatile uint8_t * input_buf
pointer to receive buffer for DMA
Definition: spi.h:149
#define IMU_MAG_X_SIGN
Definition: max7456.c:99
SPICallback before_cb
NULL or function called before the transaction.
Definition: spi.h:160
uint8_t osd_enable
Definition: max7456.c:169
struct Electrical electrical
Definition: electrical.c:66
#define MAX_PPRZ
Definition: paparazzi.h:8
uint16_t osd_char_address
Definition: max7456.c:142
Definition: max7456.c:160
uint8_t osd_spi_rx_buffer[2]
Definition: max7456.c:137
int32_t mag_y
Definition: mag_hmc5843.c:29
signed char int8_t
Definition: types.h:15
float current
current in A
Definition: electrical.h:46
uint8_t fix
status of fix
Definition: gps.h:107
int32_t mag_x
Definition: mag_hmc5843.c:29
float y
in meters
struct GpsState gps
global GPS state
Definition: gps.c:69
uint8_t mode
current autopilot mode
Definition: autopilot.h:63
float fz3
Definition: point.c:93
#define OSD_RESET_BUSY_FLAG
Definition: max7456_regs.h:60
#define OSD_NVRAM_BUSY_FLAG
Definition: max7456_regs.h:61
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
enum SPITransactionStatus status
Definition: spi.h:162
static char ascii_to_osd_c(char c)
Definition: max7456.c:369
#define IMU_MAG_Y_SENS_DEN
Definition: imu_ardrone2.h:76