PX4 Firmware
PX4 Autopilot Software http://px4.io
registers.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 registers.c
36  *
37  * Implementation of the PX4IO register space.
38  *
39  * @author Lorenz Meier <lorenz@px4.io>
40  */
41 
42 #include <px4_platform_common/px4_config.h>
43 
44 #include <stdbool.h>
45 #include <stdlib.h>
46 #include <string.h>
47 
48 #include <drivers/drv_hrt.h>
49 #include <drivers/drv_pwm_output.h>
50 #include <stm32_pwr.h>
51 #include <rc/dsm.h>
52 #include <rc/sbus.h>
53 
54 #include "px4io.h"
55 #include "protocol.h"
56 
57 static int registers_set_one(uint8_t page, uint8_t offset, uint16_t value);
58 static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate);
59 
62 
63 /**
64  * PAGE 0
65  *
66  * Static configuration parameters.
67  */
68 static const uint16_t r_page_config[] = {
78 };
79 
80 /**
81  * PAGE 1
82  *
83  * Status values.
84  */
85 volatile uint16_t r_page_status[] = {
94 };
95 
96 /**
97  * PAGE 2
98  *
99  * Post-mixed actuator values.
100  */
102 
103 /**
104  * PAGE 3
105  *
106  * Servo PWM values
107  */
109 
110 /**
111  * PAGE 4
112  *
113  * Raw RC input
114  */
115 uint16_t r_page_raw_rc_input[] = {
116  [PX4IO_P_RAW_RC_COUNT] = 0,
117  [PX4IO_P_RAW_RC_FLAGS] = 0,
118  [PX4IO_P_RAW_RC_NRSSI] = 0,
119  [PX4IO_P_RAW_RC_DATA] = 0,
122  [PX4IO_P_RAW_RC_BASE ...(PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0
123 };
124 
125 /**
126  * PAGE 5
127  *
128  * Scaled/routed RC input
129  */
130 uint16_t r_page_rc_input[] = {
131  [PX4IO_P_RC_VALID] = 0,
132  [PX4IO_P_RC_BASE ...(PX4IO_P_RC_BASE + PX4IO_RC_MAPPED_CONTROL_CHANNELS)] = 0
133 };
134 
135 /**
136  * Scratch page; used for registers that are constructed as-read.
137  *
138  * PAGE 6 Raw ADC input.
139  * PAGE 7 PWM rate maps.
140  */
141 uint16_t r_page_scratch[32];
142 
143 /**
144  * PAGE 8
145  *
146  * RAW PWM values
147  */
149 
150 /**
151  * PAGE 100
152  *
153  * Setup registers
154  */
155 volatile uint16_t r_page_setup[] = {
156  /* default to RSSI ADC functionality */
163  /* this is unused, but we will pad it for readability (the compiler pads it automatically) */
165 #ifdef ADC_VSERVO
166  [PX4IO_P_SETUP_VSERVO_SCALE] = 10000,
167 #else
168  [PX4IO_P_SETUP_VBATT_SCALE] = 10000,
169 #endif
172  [PX4IO_P_SETUP_CRC ...(PX4IO_P_SETUP_CRC + 1)] = 0,
178  [PX4IO_P_SETUP_SCALE_ROLL] = 10000,
179  [PX4IO_P_SETUP_SCALE_PITCH] = 10000,
180  [PX4IO_P_SETUP_SCALE_YAW] = 10000,
182  [PX4IO_P_SETUP_AIRMODE] = 0,
186 };
187 
188 #define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \
189  PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \
190  PX4IO_P_SETUP_FEATURES_ADC_RSSI | \
191  PX4IO_P_SETUP_FEATURES_PWM_RSSI)
192 
193 #define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \
194  PX4IO_P_SETUP_ARMING_FMU_PREARMED | \
195  PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
196  PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
197  PX4IO_P_SETUP_ARMING_IO_ARM_OK | \
198  PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
199  PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
200  PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
201  PX4IO_P_SETUP_ARMING_LOCKDOWN | \
202  PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \
203  PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE | \
204  PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE)
205 #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1)
206 #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
207 
208 /**
209  * PAGE 101
210  *
211  * Control values from the FMU.
212  */
214 
215 /*
216  * PAGE 102 does not have a buffer.
217  */
218 
219 /**
220  * PAGE 103
221  *
222  * R/C channel input configuration.
223  */
225 
226 /* valid options */
227 #define PX4IO_P_RC_CONFIG_OPTIONS_VALID (PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED)
228 
229 /*
230  * PAGE 104 uses r_page_servos.
231  */
232 
233 /**
234  * PAGE 105
235  *
236  * Failsafe servo PWM values
237  *
238  * Disable pulses as default.
239  */
240 uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 };
241 
242 /**
243  * PAGE 106
244  *
245  * minimum PWM values when armed
246  *
247  */
249 
250 /**
251  * PAGE 107
252  *
253  * maximum PWM values when armed
254  *
255  */
257 
258 /**
259  * PAGE 108
260  *
261  * trim values for center position
262  *
263  */
265 
266 /**
267  * PAGE 109
268  *
269  * disarmed PWM values for difficult ESCs
270  *
271  */
272 uint16_t r_page_servo_disarmed[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 };
273 
274 int
275 registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
276 {
277 
278  switch (page) {
279 
280  /* handle bulk controls input */
281  case PX4IO_PAGE_CONTROLS:
282 
283  /* copy channel data */
284  while ((offset < PX4IO_CONTROL_GROUPS * PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
285 
286  /* XXX range-check value? */
287  r_page_controls[offset] = *values;
288 
289  offset++;
290  num_values--;
291  values++;
292  }
293 
295 
297 
298  break;
299 
300  /* handle raw PWM input */
302 
303  /* copy channel data */
304  while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
305 
306  /* XXX range-check value? */
307  if (*values != PWM_IGNORE_THIS_CHANNEL) {
308  r_page_direct_pwm[offset] = *values;
309  }
310 
311  offset++;
312  num_values--;
313  values++;
314  }
315 
318 
319  /* Trigger all timer's channels in Oneshot mode to fire
320  * the oneshots with updated values.
321  */
322  up_pwm_update();
323 
324  break;
325 
326  /* handle setup for servo failsafe values */
328 
329  /* copy channel data */
330  while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
331 
332  if (*values == 0) {
333  /* ignore 0 */
334  } else if (*values < PWM_LOWEST_MIN) {
336 
337  } else if (*values > PWM_HIGHEST_MAX) {
339 
340  } else {
341  r_page_servo_failsafe[offset] = *values;
342  }
343 
344  /* flag the failsafe values as custom */
346 
347  offset++;
348  num_values--;
349  values++;
350  }
351 
352  break;
353 
355 
356  /* copy channel data */
357  while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
358 
359  if (*values == 0) {
360  /* ignore 0 */
361  } else if (*values > PWM_HIGHEST_MIN) {
363 
364  } else if (*values < PWM_LOWEST_MIN) {
366 
367  } else {
368  r_page_servo_control_min[offset] = *values;
369  }
370 
371  offset++;
372  num_values--;
373  values++;
374  }
375 
376  break;
377 
379 
380  /* copy channel data */
381  while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
382 
383  if (*values == 0) {
384  /* ignore 0 */
385  } else if (*values > PWM_HIGHEST_MAX) {
387 
388  } else if (*values < PWM_LOWEST_MAX) {
390 
391  } else {
392  r_page_servo_control_max[offset] = *values;
393  }
394 
395  offset++;
396  num_values--;
397  values++;
398  }
399 
400  break;
401 
403 
404  /* copy channel data */
405  while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
406 
407  r_page_servo_control_trim[offset] = *values;
408 
409  offset++;
410  num_values--;
411  values++;
412  }
413 
414  update_trims = true;
415 
416  break;
417 
419  /* flag for all outputs */
420  bool all_disarmed_off = true;
421 
422  /* copy channel data */
423  while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
424 
425  if (*values == 0) {
426  /* 0 means disabling always PWM */
427  r_page_servo_disarmed[offset] = 0;
428 
429  } else if (*values < PWM_LOWEST_MIN) {
431  all_disarmed_off = false;
432 
433  } else if (*values > PWM_HIGHEST_MAX) {
435  all_disarmed_off = false;
436 
437  } else {
438  r_page_servo_disarmed[offset] = *values;
439  all_disarmed_off = false;
440  }
441 
442  offset++;
443  num_values--;
444  values++;
445  }
446 
447  if (all_disarmed_off) {
448  /* disable PWM output if disarmed */
450 
451  } else {
452  /* enable PWM output always */
454  }
455  }
456  break;
457 
458  /* handle text going to the mixer parser */
460  /* do not change the mixer if FMU is armed and IO's safety is off
461  * this state defines an active system. This check is done in the
462  * text handling function.
463  */
464  return mixer_handle_text(values, num_values * sizeof(*values));
465 
466  default:
467 
468  /* avoid offset wrap */
469  if ((offset + num_values) > 255) {
470  num_values = 255 - offset;
471  }
472 
473  /* iterate individual registers, set each in turn */
474  while (num_values--) {
475  if (registers_set_one(page, offset, *values)) {
476  return -1;
477  }
478 
479  offset++;
480  values++;
481  }
482 
483  break;
484  }
485 
486  return 0;
487 }
488 
489 static int
490 registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
491 {
492  switch (page) {
493 
494  case PX4IO_PAGE_STATUS:
495  switch (offset) {
497  /* clear bits being written */
498  r_status_alarms &= ~value;
499  break;
500 
502 
503  /*
504  * Allow FMU override of arming state (to allow in-air restores),
505  * but only if the arming state is not in sync on the IO side.
506  */
507 
508  if (PX4IO_P_STATUS_FLAGS_MIXER_OK & value) {
510 
512  r_status_flags = value;
513 
514  }
515 
517 
518  /* update failsafe values, now that the mixer is set to ok */
520  }
521 
522  break;
523 
524  default:
525  /* just ignore writes to other registers in this page */
526  break;
527  }
528 
529  break;
530 
531  case PX4IO_PAGE_SETUP:
532  switch (offset) {
534 
536 
537  /* some of the options conflict - give S.BUS out precedence, then ADC RSSI, then PWM RSSI */
538 
539  /* switch S.Bus output pin as needed */
540 #ifdef ENABLE_SBUS_OUT
542 
543  /* disable the conflicting options with SBUS 1 */
544  if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT)) {
548  }
549 
550  /* disable the conflicting options with SBUS 2 */
551  if (value & (PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) {
555  }
556 
557 #endif
558 
559  /* disable the conflicting options with ADC RSSI */
560  if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
564  }
565 
566  /* disable the conflicting options with PWM RSSI (without effect here, but for completeness) */
567  if (value & (PX4IO_P_SETUP_FEATURES_PWM_RSSI)) {
571  }
572 
573  /* apply changes */
574  r_setup_features = value;
575 
576  break;
577 
579 
581 
582  /*
583  * Update arming state - disarm if no longer OK.
584  * This builds on the requirement that the FMU driver
585  * asks about the FMU arming state on initialization,
586  * so that an in-air reset of FMU can not lead to a
587  * lockup of the IO arming state.
588  */
589 
592  }
593 
594  /*
595  * If the failsafe termination flag is set, do not allow the autopilot to unset it
596  */
598 
599  /*
600  * If failsafe termination is enabled and force failsafe bit is set, do not allow
601  * the autopilot to clear it.
602  */
605  }
606 
607  r_setup_arming = value;
608 
609  break;
610 
612  value &= PX4IO_P_SETUP_RATES_VALID;
614  break;
615 
617  if (value < 25) {
618  value = 25;
619  }
620 
621  if (value > 400) {
622  value = 400;
623  }
624 
626  break;
627 
629 
630  /* For PWM constrain to [25,400]Hz
631  * For Oneshot there is no rate, 0 is therefore used to select Oneshot mode
632  */
633  if (value != 0) {
634  if (value < 25) {
635  value = 25;
636  }
637 
638  if (value > 400) {
639  value = 400;
640  }
641  }
642 
644  break;
645 
648  isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]);
649  break;
650 
653  // don't allow reboot while armed
654  break;
655  }
656 
657  // check the magic value
658  if (value != PX4IO_REBOOT_BL_MAGIC) {
659  break;
660  }
661 
662  // we schedule a reboot rather than rebooting
663  // immediately to allow the IO board to ACK
664  // the reboot command
665  schedule_reboot(100000);
666  break;
667 
668  case PX4IO_P_SETUP_DSM:
669  dsm_bind(value & 0x0f, (value >> 4) & 0xF);
670  break;
671 
673  if (value == PX4IO_FORCE_SAFETY_MAGIC) {
674  r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
675 
676  } else {
677  return -1;
678  }
679 
680  break;
681 
683  if (value == PX4IO_FORCE_SAFETY_MAGIC) {
685 
686  } else {
687  return -1;
688  }
689 
690  break;
691 
693  if (value > 650 && value < 2350) {
695  }
696 
697  break;
698 
700  r_page_setup[offset] = value;
702  break;
703 
705  update_mc_thrust_param = true;
706  r_page_setup[offset] = value;
707  break;
708 
720  r_page_setup[offset] = value;
721  break;
722 
723  default:
724  return -1;
725  }
726 
727  break;
728 
729  case PX4IO_PAGE_RC_CONFIG: {
730 
731  /**
732  * do not allow a RC config change while safety is off
733  */
735  break;
736  }
737 
738  unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE;
739  unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE;
740  uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE];
741 
742  if (channel >= PX4IO_RC_INPUT_CHANNELS) {
743  return -1;
744  }
745 
746  /* disable the channel until we have a chance to sanity-check it */
748 
749  switch (index) {
750 
756  conf[index] = value;
757  break;
758 
762 
763  /* clear any existing RC disabled flag */
765 
766  /* set all options except the enabled option */
767  conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
768 
769  /* should the channel be enabled? */
770  /* this option is normally set last */
771  if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
772  uint8_t count = 0;
773  bool disabled = false;
774 
775  /* assert min..center..max ordering */
776  if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) {
777  count++;
778  }
779 
780  if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) {
781  count++;
782  }
783 
785  count++;
786  }
787 
789  count++;
790  }
791 
792  /* assert deadzone is sane */
793  if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
794  count++;
795  }
796 
797  if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) {
798  disabled = true;
799 
802  count++;
803  }
804 
805  /* sanity checks pass, enable channel */
806  if (count) {
807  isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1));
809 
810  } else if (!disabled) {
811  conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
812  }
813  }
814 
815  break;
816  /* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */
817 
818  }
819 
820  break;
821  /* case PX4IO_RC_PAGE_CONFIG */
822  }
823 
824  case PX4IO_PAGE_TEST:
825  switch (offset) {
826  case PX4IO_P_TEST_LED:
827  LED_AMBER(value & 1);
828  break;
829  }
830 
831  break;
832 
833  default:
834  return -1;
835  }
836 
837  return 0;
838 }
839 
840 uint8_t last_page;
841 uint8_t last_offset;
842 
843 int
844 registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values)
845 {
846 #define SELECT_PAGE(_page_name) \
847  do { \
848  *values = (uint16_t *)&_page_name[0]; \
849  *num_values = sizeof(_page_name) / sizeof(_page_name[0]); \
850  } while(0)
851 
852  switch (page) {
853 
854  /*
855  * Handle pages that are updated dynamically at read time.
856  */
857  case PX4IO_PAGE_STATUS:
858  /* PX4IO_P_STATUS_FREEMEM */
859 
860  /* XXX PX4IO_P_STATUS_CPULOAD */
861 
862  /* PX4IO_P_STATUS_FLAGS maintained externally */
863 
864  /* PX4IO_P_STATUS_ALARMS maintained externally */
865 
866 #ifdef ADC_VBATT
867  /* PX4IO_P_STATUS_VBATT */
868  {
869  /*
870  * Coefficients here derived by measurement of the 5-16V
871  * range on one unit, validated on sample points of another unit
872  *
873  * Data in Tools/tests-host/data folder.
874  *
875  * measured slope = 0.004585267878277 (int: 4585)
876  * nominal theoretic slope: 0.00459340659 (int: 4593)
877  * intercept = 0.016646394188076 (int: 16646)
878  * nominal theoretic intercept: 0.00 (int: 0)
879  *
880  */
881  unsigned counts = adc_measure(ADC_VBATT);
882 
883  if (counts != 0xffff) {
884  unsigned mV = (166460 + (counts * 45934)) / 10000;
885  unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
886 
887  r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
888  }
889  }
890 
891 #endif
892 #ifdef ADC_IBATT
893  /* PX4IO_P_STATUS_IBATT */
894  {
895  /*
896  note that we have no idea what sort of
897  current sensor is attached, so we just
898  return the raw 12 bit ADC value and let the
899  FMU sort it out, with user selectable
900  configuration for their sensor
901  */
902  unsigned counts = adc_measure(ADC_IBATT);
903 
904  if (counts != 0xffff) {
905  r_page_status[PX4IO_P_STATUS_IBATT] = counts;
906  }
907  }
908 #endif
909 #ifdef ADC_VSERVO
910  /* PX4IO_P_STATUS_VSERVO */
911  {
912  unsigned counts = adc_measure(ADC_VSERVO);
913 
914  if (counts != 0xffff) {
915  // use 3:1 scaling on 3.3V ADC input
916  unsigned mV = counts * 9900 / 4096;
918  }
919  }
920 #endif
921 #ifdef ADC_RSSI
922  /* PX4IO_P_STATUS_VRSSI */
923  {
924  unsigned counts = adc_measure(ADC_RSSI);
925 
926  if (counts != 0xffff) {
927  // use 1:1 scaling on 3.3V ADC input
928  unsigned mV = counts * 3300 / 4096;
930  }
931  }
932 #endif
933  /* XXX PX4IO_P_STATUS_PRSSI */
934 
936  break;
937 
939  memset(r_page_scratch, 0, sizeof(r_page_scratch));
940 #ifdef ADC_VBATT
941  r_page_scratch[0] = adc_measure(ADC_VBATT);
942 #endif
943 #ifdef ADC_IBATT
944  r_page_scratch[1] = adc_measure(ADC_IBATT);
945 #endif
946 
947 #ifdef ADC_VSERVO
949 #endif
950 #ifdef ADC_RSSI
952 #endif
954  break;
955 
956  case PX4IO_PAGE_PWM_INFO:
957  memset(r_page_scratch, 0, sizeof(r_page_scratch));
958 
959  for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
961  }
962 
964  break;
965 
966  /*
967  * Pages that are just a straight read of the register state.
968  */
969 
970  /* status pages */
971  case PX4IO_PAGE_CONFIG:
973  break;
974 
977  break;
978 
979  case PX4IO_PAGE_SERVOS:
981  break;
982 
985  break;
986 
987  case PX4IO_PAGE_RC_INPUT:
989  break;
990 
991  /* readback of input pages */
992  case PX4IO_PAGE_SETUP:
994  break;
995 
996  case PX4IO_PAGE_CONTROLS:
998  break;
999 
1000  case PX4IO_PAGE_RC_CONFIG:
1002  break;
1003 
1004  case PX4IO_PAGE_DIRECT_PWM:
1006  break;
1007 
1010  break;
1011 
1014  break;
1015 
1018  break;
1019 
1022  break;
1023 
1026  break;
1027 
1028  default:
1029  return -1;
1030  }
1031 
1032 #undef SELECT_PAGE
1033 #undef COPY_PAGE
1034 
1035  last_page = page;
1036  last_offset = offset;
1037 
1038  /* if the offset is at or beyond the end of the page, we have no data */
1039  if (offset >= *num_values) {
1040  return -1;
1041  }
1042 
1043  /* correct the data pointer and count for the offset */
1044  *values += offset;
1045  *num_values -= offset;
1046 
1047  return 0;
1048 }
1049 
1050 /*
1051  * Helper function to handle changes to the PWM rate control registers.
1052  */
1053 static void
1054 pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate)
1055 {
1056  for (unsigned pass = 0; pass < 2; pass++) {
1057  for (unsigned group = 0; group < PX4IO_SERVO_COUNT; group++) {
1058 
1059  /* get the channel mask for this rate group */
1060  uint32_t mask = up_pwm_servo_get_rate_group(group);
1061 
1062  if (mask == 0) {
1063  continue;
1064  }
1065 
1066  /* all channels in the group must be either default or alt-rate */
1067  uint32_t alt = map & mask;
1068 
1069  if (pass == 0) {
1070  /* preflight */
1071  if ((alt != 0) && (alt != mask)) {
1072  /* not a legal map, bail with an alarm */
1074  return;
1075  }
1076 
1077  } else {
1078  /* set it - errors here are unexpected */
1079  if (alt != 0) {
1082  }
1083 
1084  } else {
1087  }
1088  }
1089  }
1090  }
1091  }
1092 
1093  r_setup_pwm_rates = map;
1094  r_setup_pwm_defaultrate = defaultrate;
1095  r_setup_pwm_altrate = altrate;
1096 }
#define PX4IO_P_STATUS_VSERVO
Definition: protocol.h:134
#define PWM_DEFAULT_MAX
Default maximum PWM in us.
static int registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
Definition: registers.c:490
#define PX4IO_PAGE_RAW_RC_INPUT
Definition: protocol.h:147
void schedule_reboot(uint32_t time_delta_usec)
schedule a reboot in time_delta_usec microseconds
Definition: px4io.c:246
#define PX4IO_PAGE_DISARMED_PWM
Definition: protocol.h:303
#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE
Definition: protocol.h:189
#define PX4IO_P_SETUP_FEATURES_VALID
Definition: registers.c:188
#define PX4IO_P_SETUP_TRIM_PITCH
Pitch trim, in actuator units.
Definition: protocol.h:228
#define PWM_LOWEST_MAX
Lowest PWM allowed as the maximum PWM.
#define r_status_alarms
Definition: px4io.h:108
#define PX4IO_P_CONFIG_CONTROL_COUNT
Definition: protocol.h:94
#define PX4IO_PAGE_ACTUATORS
Definition: protocol.h:141
#define PX4IO_P_CONFIG_ADC_INPUT_COUNT
Definition: protocol.h:97
#define PX4IO_P_RC_BASE
Definition: protocol.h:165
#define PX4IO_P_SETUP_FORCE_SAFETY_OFF
Definition: protocol.h:218
RC protocol definition for Spektrum RC.
#define PX4IO_P_SETUP_RELAYS_PAD
Definition: protocol.h:199
#define PX4IO_P_SETUP_SET_DEBUG
Definition: protocol.h:211
__EXPORT uint32_t up_pwm_servo_get_rate_group(unsigned group)
Get a bitmap of output channels assigned to a given rate group.
#define PX4IO_P_SETUP_PWM_DEFAULTRATE
Definition: protocol.h:197
uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT]
PAGE 107.
Definition: registers.c:256
#define PX4IO_P_RAW_FRAME_COUNT
Definition: protocol.h:158
#define PX4IO_PAGE_RC_INPUT
Definition: protocol.h:163
#define PX4IO_P_SETUP_SCALE_PITCH
Pitch scale, in actuator units.
Definition: protocol.h:231
#define PX4IO_P_SETUP_PWM_REVERSE
Bitmask to reverse PWM channels 1-8.
Definition: protocol.h:226
#define PX4IO_PAGE_FAILSAFE_PWM
0..CONFIG_ACTUATOR_COUNT-1
Definition: protocol.h:283
#define PX4IO_P_SETUP_SBUS_RATE
frame rate of SBUS1 output in Hz
Definition: protocol.h:234
uint16_t r_page_scratch[32]
Scratch page; used for registers that are constructed as-read.
Definition: registers.c:141
#define PX4IO_P_CONFIG_RC_INPUT_COUNT
Definition: protocol.h:96
int mixer_handle_text(const void *buffer, size_t length)
Definition: mixer.cpp:566
bool update_trims
Definition: registers.c:61
#define PX4IO_THERMAL_IGNORE
Definition: protocol.h:246
#define PX4IO_P_STATUS_PRSSI
Definition: protocol.h:136
#define PX4IO_P_RAW_RC_BASE
Definition: protocol.h:160
volatile uint16_t r_page_setup[]
PAGE 100.
Definition: registers.c:155
#define PX4IO_P_CONFIG_RELAY_COUNT
Definition: protocol.h:98
#define PX4IO_P_RC_VALID
Definition: protocol.h:164
int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
Register space.
Definition: registers.c:275
#define PX4IO_P_SETUP_FORCE_SAFETY_ON
Definition: protocol.h:223
void isr_debug(uint8_t level, const char *fmt,...)
send a debug message to the console
Definition: px4io.c:120
#define PX4IO_PAGE_MIXERLOAD
Definition: protocol.h:264
#define PX4IO_P_SETUP_PWM_ALTRATE
Definition: protocol.h:198
static const uint16_t r_page_config[]
PAGE 0.
Definition: registers.c:68
#define PX4IO_P_STATUS_CPULOAD
Definition: protocol.h:104
uint16_t r_page_servos[PX4IO_SERVO_COUNT]
PAGE 3.
Definition: registers.c:108
#define PX4IO_P_SETUP_CRC
Definition: protocol.h:216
#define PX4IO_RC_INPUT_CHANNELS
Definition: px4io.h:68
#define ENABLE_SBUS_OUT(_s)
Definition: px4io.h:175
#define r_setup_pwm_defaultrate
Definition: px4io.h:120
#define PX4IO_P_STATUS_ALARMS_PWM_ERROR
Definition: protocol.h:131
#define r_setup_features
Definition: px4io.h:117
#define r_setup_arming
Definition: px4io.h:118
PX4IO interface protocol.
#define PX4IO_P_SETUP_TRIM_YAW
Yaw trim, in actuator units.
Definition: protocol.h:229
High-resolution timer with callouts and timekeeping.
#define PX4IO_P_STATUS_MIXER
Definition: protocol.h:138
#define PX4IO_PAGE_TEST
Definition: protocol.h:290
uint16_t r_page_direct_pwm[PX4IO_SERVO_COUNT]
PAGE 8.
Definition: registers.c:148
#define PX4IO_RC_MAPPED_CONTROL_CHANNELS
This is the maximum number of channels mapped/used.
Definition: px4io.h:69
#define PX4IO_P_STATUS_FLAGS
Definition: protocol.h:106
#define PX4IO_P_RC_CONFIG_MAX
highest input value
Definition: protocol.h:270
#define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM
Definition: protocol.h:187
int registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values)
Definition: registers.c:844
#define PX4IO_P_SETUP_PWM_RATES
Definition: protocol.h:196
#define PX4IO_PAGE_CONTROL_MAX_PWM
0..CONFIG_ACTUATOR_COUNT-1
Definition: protocol.h:297
#define LED_AMBER
Definition: drv_board_led.h:52
#define PX4IO_P_RAW_LOST_FRAME_COUNT
Definition: protocol.h:159
#define PX4IO_P_CONFIG_MAX_TRANSFER
Definition: protocol.h:93
#define SELECT_PAGE(_page_name)
#define PX4IO_P_SETUP_ENABLE_FLIGHTTERMINATION
flight termination; false if the circuit breaker (CBRK_FLIGHTTERM) is set
Definition: protocol.h:244
#define PX4IO_P_SETUP_REBOOT_BL
Definition: protocol.h:213
#define PX4IO_PAGE_SETUP
Definition: protocol.h:175
#define PX4IO_P_RAW_RC_FLAGS
Definition: protocol.h:149
uint16_t r_page_raw_rc_input[]
PAGE 4.
Definition: registers.c:115
#define PX4IO_P_SETUP_THR_MDL_FAC
factor for modelling motor control signal output to static thrust relationship
Definition: protocol.h:238
#define PX4IO_PAGE_CONTROL_MIN_PWM
0..CONFIG_ACTUATOR_COUNT-1
Definition: protocol.h:294
#define PX4IO_P_SETUP_DSM
Definition: protocol.h:202
#define PX4IO_P_SETUP_FEATURES
Definition: protocol.h:176
uint8_t last_page
Definition: registers.c:840
#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED
Definition: protocol.h:190
uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT]
PAGE 106.
Definition: registers.c:248
#define PX4IO_PAGE_SERVOS
Definition: protocol.h:144
#define PX4IO_P_RAW_RC_COUNT
Definition: protocol.h:148
__EXPORT int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
Set the update rate for a given rate group.
#define PX4IO_P_STATUS_VRSSI
Definition: protocol.h:135
#define PX4IO_P_STATUS_FLAGS_MIXER_OK
Definition: protocol.h:115
#define r_status_flags
Definition: px4io.h:107
#define PX4IO_P_CONFIG_PROTOCOL_VERSION
Definition: protocol.h:90
#define PX4IO_P_STATUS_FLAGS_ARM_SYNC
Definition: protocol.h:116
#define PX4IO_P_SETUP_MOTOR_SLEW_MAX
max motor slew rate
Definition: protocol.h:236
#define PX4IO_P_SETUP_VSERVO_SCALE
Definition: protocol.h:201
#define PX4IO_P_RC_CONFIG_DEADZONE
band around center that is ignored
Definition: protocol.h:271
uint16_t r_page_actuators[PX4IO_SERVO_COUNT]
PAGE 2.
Definition: registers.c:101
#define PX4IO_PAGE_PWM_INFO
Definition: protocol.h:171
#define PX4IO_CONTROL_CHANNELS
Definition: px4io.h:66
#define ADC_RSSI
Definition: px4io.h:181
#define PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH
magic value for mode switch
Definition: protocol.h:273
#define PX4IO_P_CONFIG_ACTUATOR_COUNT
Definition: protocol.h:95
#define PX4IO_PAGE_RC_CONFIG
R/C input configuration.
Definition: protocol.h:267
#define PX4IO_P_TEST_LED
set the amber LED on/off
Definition: protocol.h:291
#define PX4IO_P_STATUS_FREEMEM
Definition: protocol.h:103
#define PX4IO_MAX_TRANSFER_LEN
Definition: protocol.h:99
#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF
Definition: protocol.h:119
#define PX4IO_ADC_CHANNEL_COUNT
Definition: px4io.h:179
#define PX4IO_FORCE_SAFETY_MAGIC
Definition: protocol.h:224
#define PX4IO_P_SETUP_TRIM_ROLL
Roll trim, in actuator units.
Definition: protocol.h:227
void mixer_set_failsafe()
Definition: mixer.cpp:635
#define PX4IO_P_STATUS_ALARMS
Definition: protocol.h:124
#define PX4IO_PAGE_STATUS
Definition: protocol.h:102
#define PX4IO_BL_VERSION
Definition: px4io.h:64
#define PX4IO_P_RC_CONFIG_ASSIGNMENT
mapped input value
Definition: protocol.h:272
General defines and structures for the PX4IO module firmware.
struct sys_state_s system_state
Definition: px4io.c:67
#define PX4IO_P_RC_CONFIG_STRIDE
spacing between channel config data
Definition: protocol.h:277
#define PX4IO_PAGE_CONTROL_TRIM_PWM
0..CONFIG_ACTUATOR_COUNT-1
Definition: protocol.h:300
#define PX4IO_PAGE_DIRECT_PWM
0..CONFIG_ACTUATOR_COUNT-1
Definition: protocol.h:280
#define PX4IO_P_RC_CONFIG_CENTER
center input value
Definition: protocol.h:269
#define PX4IO_RELAY_CHANNELS
Definition: px4io.h:174
#define PX4IO_REBOOT_BL_MAGIC
Definition: protocol.h:214
#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED
Definition: protocol.h:275
#define PX4IO_SERVO_COUNT
Definition: px4io.h:65
#define PWM_IGNORE_THIS_CHANNEL
Do not output a channel with this value.
#define PX4IO_P_STATUS_FLAGS_RAW_PWM
Definition: protocol.h:114
#define PX4IO_P_SETUP_THERMAL
thermal management
Definition: protocol.h:240
#define PX4IO_P_SETUP_ARMING_VALID
Definition: registers.c:193
#define PX4IO_P_STATUS_FLAGS_INIT_OK
Definition: protocol.h:117
#define PX4IO_PROTOCOL_VERSION
Definition: protocol.h:83
#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE
Definition: protocol.h:192
#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US
the throttle failsafe pulse length in microseconds
Definition: protocol.h:221
static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate)
Definition: registers.c:1054
#define PWM_HIGHEST_MIN
Highest PWM allowed as the minimum PWM.
RC protocol definition for S.BUS.
#define PX4IO_P_RC_CONFIG_OPTIONS_VALID
Definition: registers.c:227
#define PX4IO_P_SETUP_ARMING
Definition: protocol.h:182
#define PX4IO_P_SETUP_FEATURES_PWM_RSSI
enable PWM RSSI parsing
Definition: protocol.h:179
bool update_mc_thrust_param
Definition: registers.c:60
void sbus1_set_output_rate_hz(uint16_t rate_hz)
Definition: sbus.cpp:705
#define PX4IO_PAGE_CONFIG
Definition: protocol.h:89
#define PX4IO_P_SETUP_FEATURES_ADC_RSSI
enable ADC RSSI parsing
Definition: protocol.h:180
#define PX4IO_P_SETUP_SCALE_ROLL
Roll scale, in actuator units.
Definition: protocol.h:230
#define PX4IO_P_SETUP_SCALE_YAW
Yaw scale, in actuator units.
Definition: protocol.h:232
uint16_t r_page_rc_input_config[PX4IO_RC_INPUT_CHANNELS *PX4IO_P_RC_CONFIG_STRIDE]
PAGE 103.
Definition: registers.c:224
#define PX4IO_P_RAW_RC_NRSSI
Definition: protocol.h:156
#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE
Definition: protocol.h:194
#define OK
Definition: uavcan_main.cpp:71
volatile uint16_t r_page_status[]
PAGE 1.
Definition: registers.c:85
#define PX4IO_PAGE_CONTROLS
actuator control groups, one after the other, 8 wide
Definition: protocol.h:251
uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT]
PAGE 105.
Definition: registers.c:240
#define PX4IO_RATE_MAP_BASE
Definition: protocol.h:172
#define PX4IO_P_SETUP_RATES_VALID
Definition: registers.c:205
#define PWM_HIGHEST_MAX
Highest maximum PWM in us.
#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT
enable S.Bus v2 output
Definition: protocol.h:178
#define ADC_VSERVO
Definition: px4io.h:180
#define PX4IO_P_RAW_RC_DATA
Definition: protocol.h:157
#define PWM_DEFAULT_TRIM
Default trim PWM in us.
uint16_t r_page_servo_disarmed[PX4IO_SERVO_COUNT]
PAGE 109.
Definition: registers.c:272
#define r_setup_pwm_rates
Definition: px4io.h:119
#define r_setup_pwm_altrate
Definition: px4io.h:121
int16_t r_page_servo_control_trim[PX4IO_SERVO_COUNT]
PAGE 108.
Definition: registers.c:264
#define PX4IO_P_RC_CONFIG_MIN
lowest input value
Definition: protocol.h:268
#define PX4IO_P_RC_CONFIG_OPTIONS
channel options bitmask
Definition: protocol.h:274
#define PX4IO_P_CONFIG_BOOTLOADER_VERSION
Definition: protocol.h:92
#define PWM_DEFAULT_MIN
Default minimum PWM in us.
#define PX4IO_CONTROL_GROUPS
Definition: px4io.h:67
__EXPORT void up_pwm_update(void)
Trigger all timer&#39;s channels in Oneshot mode to fire the oneshot with updated values.
#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT
enable S.Bus v1 output
Definition: protocol.h:177
uint16_t r_page_rc_input[]
PAGE 5.
Definition: registers.c:130
#define PX4IO_P_SETUP_AIRMODE
air-mode
Definition: protocol.h:242
#define PX4IO_PAGE_RAW_ADC_INPUT
Definition: protocol.h:168
#define PX4IO_P_CONFIG_HARDWARE_VERSION
Definition: protocol.h:91
uint16_t adc_measure(unsigned channel)
Definition: adc.c:137
uint8_t last_offset
Definition: registers.c:841
volatile uint64_t fmu_data_received_time
Last FMU receive time, in microseconds since system boot.
Definition: px4io.h:151
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
uint16_t r_page_controls[PX4IO_CONTROL_GROUPS *PX4IO_CONTROL_CHANNELS]
PAGE 101.
Definition: registers.c:213
#define PWM_LOWEST_MIN
Lowest minimum PWM in us.
#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE
Definition: protocol.h:193