Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 11:23:06 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

Checkpoint - I2C protocol register decode

Browse Source
This commit is contained in:
px4dev
2013-01-13 02:20:01 -08:00
parent b0fb86a693
commit 8ebe21b27b
4 changed files with 369 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware.sublime-project
+1
View File
@@ -7,6 +7,7 @@
[
"*.o",
"*.a",
"*.d",
".built",
".context",
".depend",
apps/px4io/protocol.h
+11 -9
View File
@@ -62,6 +62,11 @@
* packed.
*/
#define PX4IO_CONTROL_CHANNELS 8
#define PX4IO_INPUT_CHANNELS 12
#define PX4IO_RELAY_CHANNELS 4
/* static configuration page */
#define PX4IO_PAGE_CONFIG 0
#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */
@@ -73,7 +78,6 @@
#define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */
#define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */
#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* harcoded # of relay outputs */
#define PX4IO_P_CONFIG_POWERSW_COUNT 9 /* harcoded # of switched power outputs */
/* dynamic status page */
#define PX4IO_PAGE_STATUS 1
@@ -83,9 +87,10 @@
#define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */
#define PX4IO_P_STATUS_FLAGS_ARMED (1 << 0) /* arm-ok and locally armed */
#define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */
#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 2) /* PPM input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 3) /* DSM input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 4) /* SBUS input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 4) /* DSM input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 5) /* SBUS input is valid */
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */
@@ -107,7 +112,7 @@
#define PX4IO_PAGE_RAW_RC_INPUT 4 /* 0..CONFIG_RC_INPUT_COUNT-1 */
/* array of scaled RC input values, -10000..10000 */
#define PX4IO_PAGE_RAW_RC_INPUT 5 /* 0..CONFIG_RC_INPUT_COUNT-1 */
#define PX4IO_PAGE_RC_INPUT 5 /* 0..CONFIG_RC_INPUT_COUNT-1 */
/* array of raw ADC values */
#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */
@@ -123,8 +128,7 @@
#define PX4IO_P_SETUP_PWM_LOWRATE 3 /* 'low' PWM frame output rate in Hz */
#define PX4IO_P_SETUP_PWM_HIGHRATE 4 /* 'high' PWM frame output rate in Hz */
#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay outputs, 0 = off, 1 = on */
#define PX4IO_P_SETUP_POWERSW 6 /* bitmask of switched power outputs, 0 = off, 1 = on */
#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */
/* autopilot control values, -10000..10000 */
#define PX4IO_PAGE_CONTROLS 101 /* 0..STATUS_CONTROL_COUNT */
@@ -143,8 +147,6 @@
*/
#define PX4IO_CONTROL_CHANNELS 8
#define PX4IO_INPUT_CHANNELS 12
#define PX4IO_RELAY_CHANNELS 4
#pragma pack(push, 1)
apps/px4io/px4io.h
+23 -13
View File
@@ -63,8 +63,24 @@
# define debug(fmt, args...) do {} while(0)
#endif
/*
* Registers.
*/
extern uint16_t r_page_status[]; /* PX4IO_PAGE_STATUS */
extern uint16_t r_page_actuators[]; /* PX4IO_PAGE_ACTUATORS */
extern uint16_t r_page_servos[]; /* PX4IO_PAGE_SERVOS */
extern uint16_t r_page_raw_rc_input[]; /* PX4IO_PAGE_RAW_RC_INPUT */
extern uint16_t r_page_rc_input[]; /* PX4IO_PAGE_RC_INPUT */
extern uint16_t r_page_adc[]; /* PX4IO_PAGE_RAW_ADC_INPUT */
extern volatile uint16_t r_page_setup[]; /* PX4IO_PAGE_SETUP */
extern volatile uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */
/*
* System state structure.
*
* XXX note that many fields here are deprecated and replaced by
* registers.
*/
struct sys_state_s {
@@ -128,12 +144,6 @@ struct sys_state_s {
*/
uint64_t fmu_data_received_time;
/**
* Current serial interface mode, per the serial_rx_mode parameter
* in the config packet.
*/
uint8_t serial_rx_mode;
/**
* If true, the RC signal has been lost for more than a timeout interval
*/
@@ -206,6 +216,7 @@ extern volatile int timers[TIMER_NUM_TIMERS];
#define ADC_VBATT 4
#define ADC_IN5 5
#define ADC_CHANNEL_COUNT 2
/*
* Mixer
@@ -224,6 +235,12 @@ extern void safety_init(void);
extern void comms_main(void) __attribute__((noreturn));
extern void i2c_init(void);
/*
* Register space
*/
extern void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
extern int registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values);
/*
* Sensors/misc inputs
*/
@@ -238,10 +255,3 @@ extern int dsm_init(const char *device);
extern bool dsm_input(void);
extern int sbus_init(const char *device);
extern bool sbus_input(void);
/*
* Assertion codes
*/
#define A_GPIO_OPEN_FAIL 100
#define A_SERVO_OPEN_FAIL 101
#define A_INPUTQ_OPEN_FAIL 102
apps/px4io/registers.c
+334
View File
@@ -0,0 +1,334 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file registers.c
*
* Implementation of the PX4IO register space.
*/
#include "px4io.h"
#include "protocol.h"
static int registers_set_one(uint8_t page, uint8_t offset, uint16_t value);
/**
* Setup registers
*/
uint16_t r_page_setup[] =
{
[PX4IO_P_SETUP_ARMING] = 0,
[PX4IO_P_SETUP_PWM_RATES] = 0,
[PX4IO_P_SETUP_PWM_LOWRATE] = 50,
[PX4IO_P_SETUP_PWM_HIGHRATE] = 200,
[PX4IO_P_SETUP_RELAYS] = 0,
};
#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_ARM_OK | PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE)
#define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PXIO_RELAY_CHANNELS) - 1)
/**
* Control values from the FMU.
*/
uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS];
/**
* Static configuration parameters.
*/
static const uint16_t r_page_config[] = {
[PX4IO_P_CONFIG_PROTOCOL_VERSION] = 0,
[PX4IO_P_CONFIG_SOFTWARE_VERSION] = 0,
[PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 0,
[PX4IO_P_CONFIG_MAX_TRANSFER] = 64,
[PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS,
[PX4IO_P_CONFIG_ACTUATOR_COUNT] = IO_SERVO_COUNT,
[PX4IO_P_CONFIG_RC_INPUT_COUNT] = MAX_CONTROL_CHANNELS,
[PX4IO_P_CONFIG_ADC_INPUT_COUNT] = ADC_CHANNEL_COUNT,
[PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS,
};
/**
* Status values.
*/
uint16_t r_page_status[] = {
[PX4IO_P_STATUS_FREEMEM] = 0,
[PX4IO_P_STATUS_CPULOAD] = 0,
[PX4IO_P_STATUS_FLAGS] = 0,
[PX4IO_P_STATUS_ALARMS] = 0,
[PX4IO_P_STATUS_VBATT] = 0,
[PX4IO_P_STATUS_TEMPERATURE] = 0
};
/**
* ADC input buffer.
*/
uint16_t r_page_adc[ADC_CHANNEL_COUNT];
/**
* Post-mixed actuator values.
*/
uint16_t r_page_actuators[IO_SERVO_COUNT];
/**
* Servo PWM values
*/
uint16_t r_page_servos[IO_SERVO_COUNT];
/**
* Scaled/routed RC input
*/
uint16_t r_page_rc_input[MAX_CONTROL_CHANNELS];
/**
* Raw RC input
*/
uint16_t r_page_raw_rc_input[MAX_CONTROL_CHANNELS];
void
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
{
switch (page) {
/* handle bulk controls input */
case PX4IO_PAGE_CONTROLS:
/* copy channel data */
while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
/* XXX scaling - should be -10000..10000 */
r_page_controls[offset] = *values;
offset++;
num_values--;
values++;
}
/* XXX we should cause a mixer tick ASAP */
system_state.mixer_fmu_available = true;
break;
/* handle text going to the mixer parser */
case PX4IO_PAGE_MIXERLOAD:
mixer_handle_text(values, num_values * sizeof(*values));
break;
default:
/* avoid offset wrap */
if ((offset + num_values) > 255)
num_values = 255 - offset;
/* iterate individual registers, set each in turn */
while (num_values--) {
if (registers_set_one(page, offset, *values))
break;
offset++;
values++;
}
}
}
static int
registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
{
switch (page) {
case PX4IO_PAGE_STATUS:
switch (offset) {
case PX4IO_P_STATUS_ALARMS:
/* clear bits being written */
r_page_status[PX4IO_P_STATUS_ALARMS] &= ~value;
break;
default:
/* just ignore writes to other registers in this page */
break;
}
break;
case PX4IO_PAGE_SETUP:
switch (offset) {
case PX4IO_P_SETUP_ARMING:
value &= PX4IO_P_SETUP_ARMING_VALID;
r_page_setup[PX4IO_P_SETUP_ARMING] = value;
/* update arming state - disarm if no longer OK */
if (system_state.armed && !(value & PX4IO_P_SETUP_ARMING_ARM_OK))
system_state.armed = false;
break;
case PX4IO_P_SETUP_PWM_RATES:
value &= PX4IO_P_SETUP_RATES_VALID;
r_page_setup[PX4IO_P_SETUP_PWM_RATES] = value;
/* XXX re-configure timers */
break;
case PX4IO_P_SETUP_PWM_LOWRATE:
if (value < 50)
value = 50;
if (value > 400)
value = 400;
r_page_setup[PX4IO_P_SETUP_PWM_LOWRATE] = value;
/* XXX re-configure timers */
break;
case PX4IO_P_SETUP_PWM_HIGHRATE:
if (value < 50)
value = 50;
if (value > 400)
value = 400;
r_page_setup[PX4IO_P_SETUP_PWM_HIGHRATE] = value;
/* XXX re-configure timers */
break;
case PX4IO_P_SETUP_RELAYS:
value &= PX4IO_P_SETUP_RELAYS_VALID;
r_page_setup[PX4IO_P_SETUP_RELAYS] = value;
POWER_RELAY1(value & (1 << 0) ? 1 : 0);
POWER_RELAY2(value & (1 << 1) ? 1 : 0);
POWER_ACC1(value & (1 << 2) ? 1 : 0);
POWER_ACC2(value & (1 << 3) ? 1 : 0);
break;
default:
return -1;
}
break;
default:
return -1;
}
return 0;
}
int
registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values)
{
switch (page) {
case PX4IO_PAGE_CONFIG:
*values = r_page_config;
*num_values = sizeof(r_page_config) / sizeof(r_page_config[0]);
break;
case PX4IO_PAGE_STATUS:
{
struct mallinfo minfo = mallinfo();
r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.fordblks;
}
/* XXX PX4IO_P_STATUS_CPULOAD */
r_page_status[PX4IO_P_STATUS_FLAGS] =
(system_state.armed ? PX4IO_P_STATUS_FLAGS_ARMED : 0) |
(system_state.manual_override_ok ? PX4IO_P_STATUS_FLAGS_OVERRIDE : 0) |
((system_state.rc_channels > 0) ? PX4IO_P_STATUS_FLAGS_RC_OK : 0))
/* XXX specific receiver status */
/* XXX PX4IO_P_STATUS_ALARMS] */
{
/*
* Coefficients here derived by measurement of the 5-16V
* range on one unit:
*
* V counts
* 5 1001
* 6 1219
* 7 1436
* 8 1653
* 9 1870
* 10 2086
* 11 2303
* 12 2522
* 13 2738
* 14 2956
* 15 3172
* 16 3389
*
* slope = 0.0046067
* intercept = 0.3863
*
* Intercept corrected for best results @ 12V.
*/
unsigned counts = adc_measure(ADC_VBATT);
r_page_status[PX4IO_P_STATUS_VBATT] = (4150 + (counts * 46)) / 10;
}
/* XXX PX4IO_P_STATUS_TEMPERATURE */
*values = r_page_status;
*num_values = sizeof(r_page_status) / sizeof(r_page_status[0]);
break;
case PX4IO_PAGE_ACTUATORS:
*values = r_page_actuators;
*num_values = sizeof(r_page_actuators) / sizeof(r_page_actuators[0]);
break;
case PX4IO_PAGE_SERVOS:
*values = system_state.servos;
*num_values = IO_SERVO_COUNT;
break;
case PX4IO_PAGE_RAW_RC_INPUT:
*values = r_page_raw_rc_input;
*num_values = sizeof(r_page_raw_rc_input) / sizeof(r_page_raw_rc_input[0]);
break;
case PX4IO_PAGE_RC_INPUT:
*values = system_state.rc_channel_data;
*num_values = system_state.rc_channels;
return -1;
case PX4IO_PAGE_RAW_ADC_INPUT:
r_page_adc[0] = adc_measure(ADC_VBATT);
r_page_adc[1] = adc_measure(ADC_IN5);
*values = r_page_adc;
*num_values = ADC_CHANNEL_COUNT;
break;
default:
return -1;
}
/* if the offset is beyond the end of the page, we have no data */
if (*num_values <= offset)
return -1;
/* adjust value count and pointer for the page offset */
*num_values -= offset;
*values += offset;
return 0;
}