PX4 Firmware
PX4 Autopilot Software http://px4.io
px4io.c
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file px4io.c
36  * Top-level logic for the PX4IO module.
37  *
38  * @author Lorenz Meier <lorenz@px4.io>
39  */
40 
41 #include <px4_platform_common/px4_config.h>
42 #include "platform/cxxinitialize.h"
43 
44 #include <stdio.h> // required for task_create
45 #include <stdbool.h>
46 #include <stdlib.h>
47 #include <errno.h>
48 #include <string.h>
49 #include <poll.h>
50 #include <signal.h>
51 #include <crc32.h>
52 #include <syslog.h>
53 
54 #include <drivers/drv_pwm_output.h>
55 #include <drivers/drv_hrt.h>
56 
57 #include <perf/perf_counter.h>
59 
60 #include <stm32_uart.h>
61 
62 #define DEBUG
63 #include "px4io.h"
64 
65 __EXPORT int user_start(int argc, char *argv[]);
66 
68 
69 static struct hrt_call serial_dma_call;
70 
72 
73 float dt;
74 
75 /*
76  * a set of debug buffers to allow us to send debug information from ISRs
77  */
78 
79 static volatile uint32_t msg_counter;
80 static volatile uint32_t last_msg_counter;
81 static volatile uint8_t msg_next_out, msg_next_in;
82 
83 /*
84  * WARNING: too large buffers here consume the memory required
85  * for mixer handling. Do not allocate more than 80 bytes for
86  * output.
87  */
88 #define NUM_MSG 1
89 static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE];
90 
91 static void heartbeat_blink(void);
92 static void ring_blink(void);
93 static void update_mem_usage(void);
94 
95 void atomic_modify_or(volatile uint16_t *target, uint16_t modification)
96 {
97  if ((*target | modification) != *target) {
98  PX4_CRITICAL_SECTION(*target |= modification);
99  }
100 }
101 
102 void atomic_modify_clear(volatile uint16_t *target, uint16_t modification)
103 {
104  if ((*target & ~modification) != *target) {
105  PX4_CRITICAL_SECTION(*target &= ~modification);
106  }
107 }
108 
109 void atomic_modify_and(volatile uint16_t *target, uint16_t modification)
110 {
111  if ((*target & modification) != *target) {
112  PX4_CRITICAL_SECTION(*target &= modification);
113  }
114 }
115 
116 /*
117  * add a debug message to be printed on the console
118  */
119 void
120 isr_debug(uint8_t level, const char *fmt, ...)
121 {
122  if (level > r_page_setup[PX4IO_P_SETUP_SET_DEBUG]) {
123  return;
124  }
125 
126  va_list ap;
127  va_start(ap, fmt);
128  vsnprintf(msg[msg_next_in], sizeof(msg[0]), fmt, ap);
129  va_end(ap);
130  msg_next_in = (msg_next_in + 1) % NUM_MSG;
131  msg_counter++;
132 }
133 
134 /*
135  * show all pending debug messages
136  */
137 static void
139 {
140  if (msg_counter != last_msg_counter) {
141  uint32_t n = msg_counter - last_msg_counter;
142 
143  if (n > NUM_MSG) { n = NUM_MSG; }
144 
145  last_msg_counter = msg_counter;
146 
147  while (n--) {
148  debug("%s", msg[msg_next_out]);
149  msg_next_out = (msg_next_out + 1) % NUM_MSG;
150  }
151  }
152 }
153 
154 /*
155  * Get the memory usage at 2 Hz while not armed
156  */
157 static void
159 {
160  if (/* IO armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
161  /* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
162  return;
163  }
164 
165  static uint64_t last_mem_time = 0;
166  uint64_t now = hrt_absolute_time();
167 
168  if (now - last_mem_time > (500 * 1000)) {
169  struct mallinfo minfo = mallinfo();
170  r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.fordblks;
171  last_mem_time = now;
172  }
173 }
174 
175 static void
177 {
178  static bool heartbeat = false;
179  LED_BLUE(heartbeat = !heartbeat);
180 }
181 
182 static void
184 {
185 #ifdef GPIO_LED4
186 
187  if (/* IO armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
188  /* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
189  LED_RING(1);
190  return;
191  }
192 
193  // XXX this led code does have
194  // intentionally a few magic numbers.
195  const unsigned max_brightness = 118;
196 
197  static unsigned counter = 0;
198  static unsigned brightness = max_brightness;
199  static unsigned brightness_counter = 0;
200  static unsigned on_counter = 0;
201 
202  if (brightness_counter < max_brightness) {
203 
204  bool on = ((on_counter * 100) / brightness_counter + 1) <= ((brightness * 100) / max_brightness + 1);
205 
206  // XXX once led is PWM driven,
207  // remove the ! in the line below
208  // to return to the proper breathe
209  // animation / pattern (currently inverted)
210  LED_RING(!on);
211  brightness_counter++;
212 
213  if (on) {
214  on_counter++;
215  }
216 
217  } else {
218 
219  if (counter >= 62) {
220  counter = 0;
221  }
222 
223  int n;
224 
225  if (counter < 32) {
226  n = counter;
227 
228  } else {
229  n = 62 - counter;
230  }
231 
232  brightness = (n * n) / 8;
233  brightness_counter = 0;
234  on_counter = 0;
235  counter++;
236  }
237 
238 #endif
239 }
240 
241 static uint64_t reboot_time;
242 
243 /**
244  schedule a reboot in time_delta_usec microseconds
245  */
246 void schedule_reboot(uint32_t time_delta_usec)
247 {
248  reboot_time = hrt_absolute_time() + time_delta_usec;
249 }
250 
251 /**
252  check for a scheduled reboot
253  */
254 static void check_reboot(void)
255 {
256  if (reboot_time != 0 && hrt_absolute_time() > reboot_time) {
257  up_systemreset();
258  }
259 }
260 
261 static void
263 {
264 #define APP_SIZE_MAX 0xf000
265 #define APP_LOAD_ADDRESS 0x08001000
266  // compute CRC of the current firmware
267  uint32_t sum = 0;
268 
269  for (unsigned p = 0; p < APP_SIZE_MAX; p += 4) {
270  uint32_t bytes = *(uint32_t *)(p + APP_LOAD_ADDRESS);
271  sum = crc32part((uint8_t *)&bytes, sizeof(bytes), sum);
272  }
273 
274  r_page_setup[PX4IO_P_SETUP_CRC] = sum & 0xFFFF;
275  r_page_setup[PX4IO_P_SETUP_CRC + 1] = sum >> 16;
276 }
277 
278 int
279 user_start(int argc, char *argv[])
280 {
281  /* configure the first 8 PWM outputs (i.e. all of them) */
282  up_pwm_servo_init(0xff);
283 
284 #if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE)
285 
286  /* run C++ ctors before we go any further */
287 
288  up_cxxinitialize();
289 
290 # if defined(CONFIG_SYSTEM_NSH_CXXINITIALIZE)
291 # error CONFIG_SYSTEM_NSH_CXXINITIALIZE Must not be defined! Use CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE.
292 # endif
293 
294 #else
295 # error platform is dependent on c++ both CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE must be defined.
296 #endif
297 
298  /* reset all to zero */
299  memset(&system_state, 0, sizeof(system_state));
300 
301  /* configure the high-resolution time/callout interface */
302  hrt_init();
303 
304  /* calculate our fw CRC so FMU can decide if we need to update */
306 
307  /*
308  * Poll at 1ms intervals for received bytes that have not triggered
309  * a DMA event.
310  */
311 #ifdef CONFIG_ARCH_DMA
312  hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL);
313 #endif
314 
315  /* print some startup info */
316  syslog(LOG_INFO, "\nPX4IO: starting\n");
317 
318  /* default all the LEDs to off while we start */
319  LED_AMBER(false);
320  LED_BLUE(false);
321  LED_SAFETY(false);
322 #ifdef GPIO_LED4
323  LED_RING(false);
324 #endif
325 
326  /* turn on servo power (if supported) */
327 #ifdef POWER_SERVO
328  POWER_SERVO(true);
329 #endif
330 
331  /* turn off S.Bus out (if supported) */
332 #ifdef ENABLE_SBUS_OUT
333  ENABLE_SBUS_OUT(false);
334 #endif
335 
336  /* start the safety switch handler */
337  safety_init();
338 
339  /* initialise the control inputs */
340  controls_init();
341 
342  /* set up the ADC */
343  adc_init();
344 
345  /* start the FMU interface */
346  interface_init();
347 
348  /* add a performance counter for mixing */
349  perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
350 
351  /* add a performance counter for controls */
352  perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls");
353 
354  /* and one for measuring the loop rate */
355  perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop");
356 
357  struct mallinfo minfo = mallinfo();
358  r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.mxordblk;
359  syslog(LOG_INFO, "MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);
360 
361  /* initialize PWM limit lib */
362  output_limit_init(&pwm_limit);
363 
364  /*
365  * P O L I C E L I G H T S
366  *
367  * Not enough memory, lock down.
368  *
369  * We might need to allocate mixers later, and this will
370  * ensure that a developer doing a change will notice
371  * that he just burned the remaining RAM with static
372  * allocations. We don't want him to be able to
373  * get past that point. This needs to be clearly
374  * documented in the dev guide.
375  *
376  */
377  if (minfo.mxordblk < 600) {
378 
379  syslog(LOG_ERR, "ERR: not enough MEM");
380  bool phase = false;
381 
382  while (true) {
383 
384  if (phase) {
385  LED_AMBER(true);
386  LED_BLUE(false);
387 
388  } else {
389  LED_AMBER(false);
390  LED_BLUE(true);
391  }
392 
393  up_udelay(250000);
394 
395  phase = !phase;
396  }
397  }
398 
399  /* Start the failsafe led init */
401 
402  /*
403  * Run everything in a tight loop.
404  */
405 
406  uint64_t last_debug_time = 0;
407  uint64_t last_heartbeat_time = 0;
408  uint64_t last_loop_time = 0;
409 
410  for (;;) {
411  dt = (hrt_absolute_time() - last_loop_time) / 1000000.0f;
412  last_loop_time = hrt_absolute_time();
413 
414  if (dt < 0.0001f) {
415  dt = 0.0001f;
416 
417  } else if (dt > 0.02f) {
418  dt = 0.02f;
419  }
420 
421  /* track the rate at which the loop is running */
422  perf_count(loop_perf);
423 
424  /* kick the mixer */
425  perf_begin(mixer_perf);
426  mixer_tick();
427  perf_end(mixer_perf);
428 
429  /* kick the control inputs */
430  perf_begin(controls_perf);
431  controls_tick();
432  perf_end(controls_perf);
433 
434  /* some boards such as Pixhawk 2.1 made
435  the unfortunate choice to combine the blue led channel with
436  the IMU heater. We need a software hack to fix the hardware hack
437  by allowing to disable the LED / heater.
438  */
440  /*
441  blink blue LED at 4Hz in normal operation. When in
442  override blink 4x faster so the user can clearly see
443  that override is happening. This helps when
444  pre-flight testing the override system
445  */
446  uint32_t heartbeat_period_us = 250 * 1000UL;
447 
449  heartbeat_period_us /= 4;
450  }
451 
452  if ((hrt_absolute_time() - last_heartbeat_time) > heartbeat_period_us) {
453  last_heartbeat_time = hrt_absolute_time();
454  heartbeat_blink();
455  }
456 
458  /* switch resistive heater off */
459  LED_BLUE(false);
460 
461  } else {
462  /* switch resistive heater hard on */
463  LED_BLUE(true);
464  }
465 
467 
468  ring_blink();
469 
470  check_reboot();
471 
472  /* check for debug activity (default: none) */
474 
475  /* post debug state at ~1Hz - this is via an auxiliary serial port
476  * DEFAULTS TO OFF!
477  */
478  if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {
479 
480  isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u",
482  (unsigned)r_status_flags,
483  (unsigned)r_setup_arming,
484  (unsigned)r_setup_features,
485  (unsigned)mallinfo().mxordblk);
486  last_debug_time = hrt_absolute_time();
487  }
488  }
489 }
490 
#define PX4_CRITICAL_SECTION(cmd)
Definition: px4io.h:187
void schedule_reboot(uint32_t time_delta_usec)
schedule a reboot in time_delta_usec microseconds
Definition: px4io.c:246
static struct hrt_call serial_dma_call
Definition: px4io.c:69
volatile uint16_t r_page_setup[]
PAGE 100.
Definition: registers.c:155
measure the time elapsed performing an event
Definition: perf_counter.h:56
#define PX4IO_P_SETUP_SET_DEBUG
Definition: protocol.h:211
void failsafe_led_init(void)
Definition: safety.c:91
static void check_reboot(void)
check for a scheduled reboot
Definition: px4io.c:254
#define debug(fmt, args...)
#define PX4IO_THERMAL_IGNORE
Definition: protocol.h:246
void(* hrt_callout)(void *arg)
Callout function type.
Definition: drv_hrt.h:67
void controls_init(void)
R/C receiver handling.
Definition: controls.c:173
Definition: I2C.hpp:51
void isr_debug(uint8_t level, const char *fmt,...)
send a debug message to the console
Definition: px4io.c:120
static volatile uint32_t last_msg_counter
Definition: px4io.c:80
#define PX4IO_P_SETUP_CRC
Definition: protocol.h:216
static uint64_t reboot_time
Definition: px4io.c:241
#define ENABLE_SBUS_OUT(_s)
Definition: px4io.h:175
#define r_setup_features
Definition: px4io.h:117
#define r_setup_arming
Definition: px4io.h:118
High-resolution timer with callouts and timekeeping.
void atomic_modify_clear(volatile uint16_t *target, uint16_t modification)
Definition: px4io.c:102
static void heartbeat_blink(void)
Definition: px4io.c:176
__EXPORT int user_start(int argc, char *argv[])
Definition: px4io.c:279
static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE]
Definition: px4io.c:89
#define APP_LOAD_ADDRESS
#define LED_AMBER
Definition: drv_board_led.h:52
#define LED_BLUE
Definition: drv_board_led.h:54
static volatile uint8_t msg_next_out
Definition: px4io.c:81
void perf_count(perf_counter_t handle)
Count a performance event.
#define LED_SAFETY
Definition: drv_board_led.h:55
Header common to all counters.
#define perf_alloc(a, b)
Definition: px4io.h:59
#define APP_SIZE_MAX
void atomic_modify_or(volatile uint16_t *target, uint16_t modification)
Definition: px4io.c:95
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
static volatile uint8_t msg_next_in
Definition: px4io.c:81
#define r_status_flags
Definition: px4io.h:107
void safety_init(void)
Safety switch/LED.
Definition: safety.c:84
#define NUM_MSG
Definition: px4io.c:88
static unsigned counter
Definition: safety.c:56
void perf_end(perf_counter_t handle)
End a performance event.
void mixer_tick()
Definition: mixer.cpp:117
void interface_init(void)
FMU communications.
Definition: serial.c:90
static void update_mem_usage(void)
Definition: px4io.c:158
__EXPORT void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
Call callout(arg) after delay, and then after every interval.
#define PX4IO_P_STATUS_FLAGS_OVERRIDE
Definition: protocol.h:108
#define PX4IO_P_STATUS_FREEMEM
Definition: protocol.h:103
#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF
Definition: protocol.h:119
output_limit_t pwm_limit
Definition: px4io.c:71
General defines and structures for the PX4IO module firmware.
struct sys_state_s system_state
Definition: px4io.c:67
#define PX4IO_P_SETUP_THERMAL
thermal management
Definition: protocol.h:240
#define PX4IO_P_SETUP_ARMING_FMU_ARMED
Definition: protocol.h:184
#define PX4IO_THERMAL_FULL
Definition: protocol.h:248
float dt
Definition: px4io.c:73
void output_limit_init(output_limit_t *limit)
__EXPORT int up_pwm_servo_init(uint32_t channel_mask)
Intialise the PWM servo outputs using the specified configuration.
static volatile uint32_t msg_counter
Definition: px4io.c:79
#define LED_RING(_s)
Definition: px4io.h:171
volatile uint16_t r_page_status[]
PAGE 1.
Definition: registers.c:85
static void calculate_fw_crc(void)
Definition: px4io.c:262
Callout record.
Definition: drv_hrt.h:72
static void ring_blink(void)
Definition: px4io.c:183
static void show_debug_messages(void)
Definition: px4io.c:138
void atomic_modify_and(volatile uint16_t *target, uint16_t modification)
Definition: px4io.c:109
void controls_tick()
Definition: controls.c:205
__EXPORT void hrt_init(void)
Library for output limiting (PWM for example)
int adc_init(void)
Sensors/misc inputs.
Definition: adc.c:82
void perf_begin(perf_counter_t handle)
Begin a performance event.
measure the interval between instances of an event
Definition: perf_counter.h:57
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
Performance measuring tools.