Browse Source

px4iofirmware: support setting the SBUS output frame rate

this allows for helicopters and multicopters with SBUS output
sbg
Andrew Tridgell 9 years ago committed by Lorenz Meier
parent
commit
f19761d278
  1. 2
      src/modules/px4iofirmware/protocol.h
  2. 1
      src/modules/px4iofirmware/px4io.h
  3. 6
      src/modules/px4iofirmware/registers.c

2
src/modules/px4iofirmware/protocol.h

@ -239,6 +239,8 @@ enum { /* DSM bind states */ @@ -239,6 +239,8 @@ enum { /* DSM bind states */
#define PX4IO_P_SETUP_TRIM_PITCH 17 /**< Pitch trim, in actuator units */
#define PX4IO_P_SETUP_TRIM_YAW 18 /**< Yaw trim, in actuator units */
#define PX4IO_P_SETUP_SBUS_RATE 19 /* frame rate of SBUS1 output in Hz */
/* autopilot control values, -10000..10000 */
#define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */
#define PX4IO_P_CONTROLS_GROUP_0 (PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0) /**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */

1
src/modules/px4iofirmware/px4io.h

@ -122,6 +122,7 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ @@ -122,6 +122,7 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
#define r_setup_trim_roll r_page_setup[PX4IO_P_SETUP_TRIM_ROLL]
#define r_setup_trim_pitch r_page_setup[PX4IO_P_SETUP_TRIM_PITCH]
#define r_setup_trim_yaw r_page_setup[PX4IO_P_SETUP_TRIM_YAW]
#define r_setup_sbus_rate r_page_setup[PX4IO_P_SETUP_SBUS_RATE]
#define r_control_values (&r_page_controls[0])

6
src/modules/px4iofirmware/registers.c

@ -48,6 +48,7 @@ @@ -48,6 +48,7 @@
#include <systemlib/systemlib.h>
#include <stm32_pwr.h>
#include <rc/dsm.h>
#include <rc/sbus.h>
#include "px4io.h"
#include "protocol.h"
@ -157,6 +158,7 @@ volatile uint16_t r_page_setup[] = { @@ -157,6 +158,7 @@ volatile uint16_t r_page_setup[] = {
[PX4IO_P_SETUP_PWM_RATES] = 0,
[PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50,
[PX4IO_P_SETUP_PWM_ALTRATE] = 200,
[PX4IO_P_SETUP_SBUS_RATE] = 72,
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
[PX4IO_P_SETUP_RELAYS] = 0,
#else
@ -682,6 +684,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) @@ -682,6 +684,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
r_page_setup[offset] = value;
break;
case PX4IO_P_SETUP_SBUS_RATE:
sbus1_set_output_rate_hz(value);
break;
default:
return -1;
}

Loading…
Cancel
Save