fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 05:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into seatbelt_multirotor

Browse Source
This commit is contained in:
Anton Babushkin
2013-07-11 20:49:28 +04:00
parent 04fbed493a 3ac760978f
commit c291c4482a
43 changed files with 1198 additions and 811 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/gpio_led/gpio_led.c
+85 -22
View File
@@ -53,10 +53,12 @@
#include <uORB/topics/vehicle_status.h>
#include <poll.h>
#include <drivers/drv_gpio.h>
#include <modules/px4iofirmware/protocol.h>
struct gpio_led_s {
struct work_s work;
int gpio_fd;
bool use_io;
int pin;
struct vehicle_status_s status;
int vehicle_status_sub;
@@ -75,51 +77,97 @@ void gpio_led_cycle(FAR void *arg);
int gpio_led_main(int argc, char *argv[])
{
int pin = GPIO_EXT_1;
if (argc < 2) {
errx(1, "no argument provided. Try 'start' or 'stop' [-p 1/2]");
errx(1, "usage: gpio_led {start|stop} [-p <1|2|a1|a2|r1|r2>]\n"
"\t-p\tUse pin:\n"
"\t\t1\tPX4FMU GPIO_EXT1 (default)\n"
"\t\t2\tPX4FMU GPIO_EXT2\n"
"\t\ta1\tPX4IO ACC1\n"
"\t\ta2\tPX4IO ACC2\n"
"\t\tr1\tPX4IO RELAY1\n"
"\t\tr2\tPX4IO RELAY2");
} else {
/* START COMMAND HANDLING */
if (!strcmp(argv[1], "start")) {
if (gpio_led_started) {
errx(1, "already running");
}
bool use_io = false;
int pin = GPIO_EXT_1;
if (argc > 2) {
if (!strcmp(argv[1], "-p")) {
if (!strcmp(argv[2], "1")) {
if (!strcmp(argv[2], "-p")) {
if (!strcmp(argv[3], "1")) {
use_io = false;
pin = GPIO_EXT_1;
} else if (!strcmp(argv[2], "2")) {
} else if (!strcmp(argv[3], "2")) {
use_io = false;
pin = GPIO_EXT_2;
} else if (!strcmp(argv[3], "a1")) {
use_io = true;
pin = PX4IO_ACC1;
} else if (!strcmp(argv[3], "a2")) {
use_io = true;
pin = PX4IO_ACC2;
} else if (!strcmp(argv[3], "r1")) {
use_io = true;
pin = PX4IO_RELAY1;
} else if (!strcmp(argv[3], "r2")) {
use_io = true;
pin = PX4IO_RELAY2;
} else {
warnx("[gpio_led] Unsupported pin: %s\n", argv[2]);
exit(1);
errx(1, "unsupported pin: %s", argv[3]);
}
}
}
memset(&gpio_led_data, 0, sizeof(gpio_led_data));
gpio_led_data.use_io = use_io;
gpio_led_data.pin = pin;
int ret = work_queue(LPWORK, &gpio_led_data.work, gpio_led_start, &gpio_led_data, 0);
if (ret != 0) {
warnx("[gpio_led] Failed to queue work: %d\n", ret);
exit(1);
errx(1, "failed to queue work: %d", ret);
} else {
gpio_led_started = true;
char pin_name[24];
if (use_io) {
if (pin & (PX4IO_ACC1 | PX4IO_ACC2)) {
sprintf(pin_name, "PX4IO ACC%i", (pin >> 3));
} else {
sprintf(pin_name, "PX4IO RELAY%i", pin);
}
} else {
sprintf(pin_name, "PX4FMU GPIO_EXT%i", pin);
}
warnx("start, using pin: %s", pin_name);
}
exit(0);
/* STOP COMMAND HANDLING */
} else if (!strcmp(argv[1], "stop")) {
gpio_led_started = false;
if (gpio_led_started) {
gpio_led_started = false;
warnx("stop");
/* INVALID COMMAND */
} else {
errx(1, "not running");
}
} else {
errx(1, "unrecognized command '%s', only supporting 'start' or 'stop'", argv[1]);
@@ -131,15 +179,26 @@ void gpio_led_start(FAR void *arg)
{
FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg;
char *gpio_dev;
if (priv->use_io) {
gpio_dev = PX4IO_DEVICE_PATH;
} else {
gpio_dev = PX4FMU_DEVICE_PATH;
}
/* open GPIO device */
priv->gpio_fd = open(GPIO_DEVICE_PATH, 0);
priv->gpio_fd = open(gpio_dev, 0);
if (priv->gpio_fd < 0) {
warnx("[gpio_led] GPIO: open fail\n");
// TODO find way to print errors
//printf("gpio_led: GPIO device \"%s\" open fail\n", gpio_dev);
gpio_led_started = false;
return;
}
/* configure GPIO pin */
/* px4fmu only, px4io doesn't support GPIO_SET_OUTPUT and will ignore */
ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin);
/* subscribe to vehicle status topic */
@@ -150,11 +209,11 @@ void gpio_led_start(FAR void *arg)
int ret = work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, 0);
if (ret != 0) {
warnx("[gpio_led] Failed to queue work: %d\n", ret);
// TODO find way to print errors
//printf("gpio_led: failed to queue work: %d\n", ret);
gpio_led_started = false;
return;
}
warnx("[gpio_led] Started, using pin GPIO_EXT%i\n", priv->pin);
}
void gpio_led_cycle(FAR void *arg)
@@ -200,7 +259,6 @@ void gpio_led_cycle(FAR void *arg)
if (led_state_new) {
ioctl(priv->gpio_fd, GPIO_SET, priv->pin);
} else {
ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);
}
@@ -211,7 +269,12 @@ void gpio_led_cycle(FAR void *arg)
if (priv->counter > 5)
priv->counter = 0;
/* repeat cycle at 5 Hz*/
if (gpio_led_started)
/* repeat cycle at 5 Hz */
if (gpio_led_started) {
work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(200000));
} else {
/* switch off LED on stop */
ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);
}
}
src/modules/mavlink/mavlink.c
+3 -4
View File
@@ -49,7 +49,6 @@
#include <mqueue.h>
#include <string.h>
#include "mavlink_bridge_header.h"
#include <v1.0/common/mavlink.h>
#include <drivers/drv_hrt.h>
#include <time.h>
#include <float.h>
@@ -471,7 +470,7 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf
}
void
mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length)
mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
{
write(uart, ch, (size_t)(sizeof(uint8_t) * length));
}
@@ -479,7 +478,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length)
/*
* Internal function to give access to the channel status for each channel
*/
mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
extern mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
{
static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
return &m_mavlink_status[channel];
@@ -488,7 +487,7 @@ mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
/*
* Internal function to give access to the channel buffer for each channel
*/
mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel)
extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel)
{
static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
return &m_mavlink_buffer[channel];
src/modules/mavlink/mavlink_bridge_header.h
+5 -3
View File
@@ -73,9 +73,11 @@ extern mavlink_system_t mavlink_system;
* @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0
* @param ch Character to send
*/
extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int length);
extern void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length);
mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
#include <v1.0/common/mavlink.h>
#endif /* MAVLINK_BRIDGE_HEADER_H */
src/modules/mavlink/mavlink_parameters.c
-1
View File
@@ -40,7 +40,6 @@
*/
#include "mavlink_bridge_header.h"
#include <v1.0/common/mavlink.h>
#include "mavlink_parameters.h"
#include <uORB/uORB.h>
#include "math.h" /* isinf / isnan checks */
src/modules/mavlink/mavlink_receiver.cpp
-2
View File
@@ -49,7 +49,6 @@
#include <fcntl.h>
#include <mqueue.h>
#include <string.h>
#include <v1.0/common/mavlink.h>
#include <drivers/drv_hrt.h>
#include <time.h>
#include <float.h>
@@ -503,7 +502,6 @@ handle_message(mavlink_message_t *msg)
} else {
orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
}
warnx("IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s);
hil_global_pos.lat = hil_state.lat;
hil_global_pos.lon = hil_state.lon;
src/modules/mavlink/missionlib.c
-1
View File
@@ -48,7 +48,6 @@
#include <mqueue.h>
#include <string.h>
#include "mavlink_bridge_header.h"
#include <v1.0/common/mavlink.h>
#include <drivers/drv_hrt.h>
#include <time.h>
#include <float.h>
src/modules/mavlink/missionlib.h
+1 -1
View File
@@ -39,7 +39,7 @@
#pragma once
#include <v1.0/common/mavlink.h>
#include "mavlink_bridge_header.h"
//extern void mavlink_wpm_send_message(mavlink_message_t *msg);
//extern void mavlink_wpm_send_gcs_string(const char *string);
src/modules/mavlink/orb_listener.c
-1
View File
@@ -47,7 +47,6 @@
#include <fcntl.h>
#include <string.h>
#include "mavlink_bridge_header.h"
#include <v1.0/common/mavlink.h>
#include <drivers/drv_hrt.h>
#include <time.h>
#include <float.h>
src/modules/mavlink/waypoints.c
+1
View File
@@ -45,6 +45,7 @@
#include <unistd.h>
#include <stdio.h>
#include "mavlink_bridge_header.h"
#include "missionlib.h"
#include "waypoints.h"
#include "util.h"
src/modules/mavlink/waypoints.h
+5 -5
View File
@@ -47,11 +47,11 @@
#include <v1.0/mavlink_types.h>
#ifndef MAVLINK_SEND_UART_BYTES
#define MAVLINK_SEND_UART_BYTES(chan, buffer, len) mavlink_send_uart_bytes(chan, buffer, len)
#endif
extern mavlink_system_t mavlink_system;
#include <v1.0/common/mavlink.h>
// #ifndef MAVLINK_SEND_UART_BYTES
// #define MAVLINK_SEND_UART_BYTES(chan, buffer, len) mavlink_send_uart_bytes(chan, buffer, len)
// #endif
//extern mavlink_system_t mavlink_system;
#include "mavlink_bridge_header.h"
#include <stdbool.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
src/modules/mavlink_onboard/mavlink.c
-1
View File
@@ -49,7 +49,6 @@
#include <mqueue.h>
#include <string.h>
#include "mavlink_bridge_header.h"
#include <v1.0/common/mavlink.h>
#include <drivers/drv_hrt.h>
#include <time.h>
#include <float.h>
src/modules/mavlink_onboard/mavlink_bridge_header.h
+2
View File
@@ -78,4 +78,6 @@ extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int len
mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan);
#include <v1.0/common/mavlink.h>
#endif /* MAVLINK_BRIDGE_HEADER_H */
src/modules/mavlink_onboard/mavlink_receiver.c
-1
View File
@@ -50,7 +50,6 @@
#include <mqueue.h>
#include <string.h>
#include "mavlink_bridge_header.h"
#include <v1.0/common/mavlink.h>
#include <drivers/drv_hrt.h>
#include <time.h>
#include <float.h>
src/modules/px4iofirmware/controls.c
+9 -2
View File
@@ -95,9 +95,16 @@ controls_tick() {
*/
perf_begin(c_gather_dsm);
bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count);
if (dsm_updated)
uint16_t temp_count = r_raw_rc_count;
bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
if (dsm_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
r_raw_rc_count = temp_count & 0x7fff;
if (temp_count & 0x8000)
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11;
else
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11;
}
perf_end(c_gather_dsm);
perf_begin(c_gather_sbus);
src/modules/px4iofirmware/dsm.c
+40 -1
View File
@@ -40,6 +40,7 @@
*/
#include <nuttx/config.h>
#include <nuttx/arch.h>
#include <fcntl.h>
#include <unistd.h>
@@ -101,6 +102,41 @@ dsm_init(const char *device)
return dsm_fd;
}
void
dsm_bind(uint16_t cmd, int pulses)
{
const uint32_t usart1RxAsOutp = GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10;
if (dsm_fd < 0)
return;
switch (cmd) {
case dsm_bind_power_down:
// power down DSM satellite
POWER_RELAY1(0);
break;
case dsm_bind_power_up:
POWER_RELAY1(1);
dsm_guess_format(true);
break;
case dsm_bind_set_rx_out:
stm32_configgpio(usart1RxAsOutp);
break;
case dsm_bind_send_pulses:
for (int i = 0; i < pulses; i++) {
stm32_gpiowrite(usart1RxAsOutp, false);
up_udelay(50);
stm32_gpiowrite(usart1RxAsOutp, true);
up_udelay(50);
}
break;
case dsm_bind_reinit_uart:
// Restore USART rx pin
stm32_configgpio(GPIO_USART1_RX);
break;
}
}
bool
dsm_input(uint16_t *values, uint16_t *num_values)
{
@@ -218,7 +254,7 @@ dsm_guess_format(bool reset)
/*
* Iterate the set of sensible sniffed channel sets and see whether
* decoding in 10 or 11-bit mode has yielded anything we recognise.
* decoding in 10 or 11-bit mode has yielded anything we recognize.
*
* XXX Note that due to what seem to be bugs in the DSM2 high-resolution
* stream, we may want to sniff for longer in some cases when we think we
@@ -349,6 +385,9 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
values[channel] = value;
}
if (channel_shift == 11)
*num_values |= 0x8000;
/*
* XXX Note that we may be in failsafe here; we need to work out how to detect that.
*/
src/modules/px4iofirmware/protocol.h
+16
View File
@@ -105,6 +105,7 @@
#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */
#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */
#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */
#define PX4IO_P_STATUS_FLAGS_RC_DSM11 (1 << 12) /* DSM input is 11 bit data */
#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 */
@@ -156,7 +157,22 @@
#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */
#define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */
#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */
/* px4io relay bit definitions */
#define PX4IO_RELAY1 (1<<0)
#define PX4IO_RELAY2 (1<<1)
#define PX4IO_ACC1 (1<<2)
#define PX4IO_ACC2 (1<<3)
#define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */
#define PX4IO_P_SETUP_DSM 7 /* DSM bind state */
enum { /* DSM bind states */
dsm_bind_power_down = 0,
dsm_bind_power_up,
dsm_bind_set_rx_out,
dsm_bind_send_pulses,
dsm_bind_reinit_uart
};
#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
/* autopilot control values, -10000..10000 */
src/modules/px4iofirmware/px4io.h
+1
View File
@@ -184,6 +184,7 @@ extern void controls_init(void);
extern void controls_tick(void);
extern int dsm_init(const char *device);
extern bool dsm_input(uint16_t *values, uint16_t *num_values);
extern void dsm_bind(uint16_t cmd, int pulses);
extern int sbus_init(const char *device);
extern bool sbus_input(uint16_t *values, uint16_t *num_values);
src/modules/px4iofirmware/registers.c
+8 -4
View File
@@ -349,10 +349,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
case PX4IO_P_SETUP_RELAYS:
value &= PX4IO_P_SETUP_RELAYS_VALID;
r_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);
POWER_RELAY1(value & PX4IO_RELAY1 ? 1 : 0);
POWER_RELAY2(value & PX4IO_RELAY2 ? 1 : 0);
POWER_ACC1(value & PX4IO_ACC1 ? 1 : 0);
POWER_ACC2(value & PX4IO_ACC2 ? 1 : 0);
break;
case PX4IO_P_SETUP_SET_DEBUG:
@@ -360,6 +360,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]);
break;
case PX4IO_P_SETUP_DSM:
dsm_bind(value & 0x0f, (value >> 4) & 7);
break;
default:
return -1;
}
src/modules/sdlog2/sdlog2.c
+33 -1
View File
@@ -79,6 +79,7 @@
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/esc_status.h>
#include <systemlib/systemlib.h>
@@ -614,7 +615,7 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* number of messages */
const ssize_t fdsc = 17;
const ssize_t fdsc = 18;
/* Sanity check variable and index */
ssize_t fdsc_count = 0;
/* file descriptors to wait for */
@@ -642,6 +643,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct rc_channels_s rc;
struct differential_pressure_s diff_pres;
struct airspeed_s airspeed;
struct esc_status_s esc;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -663,6 +665,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int flow_sub;
int rc_sub;
int airspeed_sub;
int esc_sub;
} subs;
/* log message buffer: header + body */
@@ -686,6 +689,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_ARSP_s log_ARSP;
struct log_FLOW_s log_FLOW;
struct log_GPOS_s log_GPOS;
struct log_ESC_s log_ESC;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@@ -795,6 +799,12 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- ESCs --- */
subs.esc_sub = orb_subscribe(ORB_ID(esc_status));
fds[fdsc_count].fd = subs.esc_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* WARNING: If you get the error message below,
* then the number of registered messages (fdsc)
* differs from the number of messages in the above list.
@@ -1105,6 +1115,28 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(AIRS);
}
/* --- ESCs --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(esc_status), subs.esc_sub, &buf.esc);
for (uint8_t i=0; i<buf.esc.esc_count; i++)
{
log_msg.msg_type = LOG_ESC_MSG;
log_msg.body.log_ESC.counter = buf.esc.counter;
log_msg.body.log_ESC.esc_count = buf.esc.esc_count;
log_msg.body.log_ESC.esc_connectiontype = buf.esc.esc_connectiontype;
log_msg.body.log_ESC.esc_num = i;
log_msg.body.log_ESC.esc_address = buf.esc.esc[i].esc_address;
log_msg.body.log_ESC.esc_version = buf.esc.esc[i].esc_version;
log_msg.body.log_ESC.esc_voltage = buf.esc.esc[i].esc_voltage;
log_msg.body.log_ESC.esc_current = buf.esc.esc[i].esc_current;
log_msg.body.log_ESC.esc_rpm = buf.esc.esc[i].esc_rpm;
log_msg.body.log_ESC.esc_temperature = buf.esc.esc[i].esc_temperature;
log_msg.body.log_ESC.esc_setpoint = buf.esc.esc[i].esc_setpoint;
log_msg.body.log_ESC.esc_setpoint_raw = buf.esc.esc[i].esc_setpoint_raw;
LOGBUFFER_WRITE_AND_COUNT(ESC);
}
}
#ifdef SDLOG2_DEBUG
printf("fill rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb));
#endif
src/modules/sdlog2/sdlog2_messages.h
+19
View File
@@ -209,6 +209,24 @@ struct log_GPOS_s {
float vel_e;
float vel_d;
};
/* --- ESC - ESC STATE --- */
#define LOG_ESC_MSG 64
struct log_ESC_s {
uint16_t counter;
uint8_t esc_count;
uint8_t esc_connectiontype;
uint8_t esc_num;
uint16_t esc_address;
uint16_t esc_version;
uint16_t esc_voltage;
uint16_t esc_current;
uint16_t esc_rpm;
uint16_t esc_temperature;
float esc_setpoint;
uint16_t esc_setpoint_raw;
};
#pragma pack(pop)
/* construct list of all message formats */
@@ -230,6 +248,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,No,Version,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
};
static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
src/modules/sensors/sensor_params.c
+1
View File
@@ -155,6 +155,7 @@ PARAM_DEFINE_FLOAT(RC14_REV, 1.0f);
PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f);
PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */
PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f));
src/modules/sensors/sensors.cpp
+12
View File
@@ -230,6 +230,8 @@ private:
float rc_scale_flaps;
float battery_voltage_scaling;
int rc_rl1_DSM_VCC_control;
} _parameters; /**< local copies of interesting parameters */
struct {
@@ -278,6 +280,8 @@ private:
param_t rc_scale_flaps;
param_t battery_voltage_scaling;
param_t rc_rl1_DSM_VCC_control;
} _parameter_handles; /**< handles for interesting parameters */
@@ -514,6 +518,9 @@ Sensors::Sensors() :
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
/* DSM VCC relay control */
_parameter_handles.rc_rl1_DSM_VCC_control = param_find("RC_RL1_DSM_VCC");
/* fetch initial parameter values */
parameters_update();
}
@@ -730,6 +737,11 @@ Sensors::parameters_update()
warnx("Failed updating voltage scaling param");
}
/* relay 1 DSM VCC control */
if (param_get(_parameter_handles.rc_rl1_DSM_VCC_control, &(_parameters.rc_rl1_DSM_VCC_control)) != OK) {
warnx("Failed updating relay 1 DSM VCC control");
}
return OK;
}
src/modules/systemlib/cpuload.c
+20 -20
View File
@@ -71,8 +71,6 @@ extern FAR struct _TCB *sched_gettcb(pid_t pid);
void cpuload_initialize_once()
{
// if (!system_load.initialized)
// {
system_load.start_time = hrt_absolute_time();
int i;
@@ -80,27 +78,29 @@ void cpuload_initialize_once()
system_load.tasks[i].valid = false;
}
system_load.total_count = 0;
uint64_t now = hrt_absolute_time();
/* initialize idle thread statically */
system_load.tasks[0].start_time = now;
system_load.tasks[0].total_runtime = 0;
system_load.tasks[0].curr_start_time = 0;
system_load.tasks[0].tcb = sched_gettcb(0);
system_load.tasks[0].valid = true;
system_load.total_count++;
int static_tasks_count = 2; // there are at least 2 threads that should be initialized statically - "idle" and "init"
/* initialize init thread statically */
system_load.tasks[1].start_time = now;
system_load.tasks[1].total_runtime = 0;
system_load.tasks[1].curr_start_time = 0;
system_load.tasks[1].tcb = sched_gettcb(1);
system_load.tasks[1].valid = true;
/* count init thread */
system_load.total_count++;
// }
#ifdef CONFIG_PAGING
static_tasks_count++; // include paging thread in initialization
#endif /* CONFIG_PAGING */
#if CONFIG_SCHED_WORKQUEUE
static_tasks_count++; // include high priority work0 thread in initialization
#endif /* CONFIG_SCHED_WORKQUEUE */
#if CONFIG_SCHED_LPWORK
static_tasks_count++; // include low priority work1 thread in initialization
#endif /* CONFIG_SCHED_WORKQUEUE */
// perform static initialization of "system" threads
for (system_load.total_count = 0; system_load.total_count < static_tasks_count; system_load.total_count++)
{
system_load.tasks[system_load.total_count].start_time = now;
system_load.tasks[system_load.total_count].total_runtime = 0;
system_load.tasks[system_load.total_count].curr_start_time = 0;
system_load.tasks[system_load.total_count].tcb = sched_gettcb(system_load.total_count); // it is assumed that these static threads have consecutive PIDs
system_load.tasks[system_load.total_count].valid = true;
}
}
void sched_note_start(FAR struct tcb_s *tcb)
src/modules/uORB/objects_common.cpp
+3
View File
@@ -169,3 +169,6 @@ ORB_DEFINE(debug_key_value, struct debug_key_value_s);
#include "topics/navigation_capabilities.h"
ORB_DEFINE(navigation_capabilities, struct navigation_capabilities_s);
#include "topics/esc_status.h"
ORB_DEFINE(esc_status, struct esc_status_s);
src/modules/uORB/topics/esc_status.h
+114
View File
@@ -0,0 +1,114 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: @author Marco Bauer <marco@wtns.de>
*
* 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 esc_status.h
* Definition of the esc_status uORB topic.
*
* Published the state machine and the system status bitfields
* (see SYS_STATUS mavlink message), published only by commander app.
*
* All apps should write to subsystem_info:
*
* (any app) --> subsystem_info (published) --> (commander app state machine) --> esc_status --> (mavlink app)
*/
#ifndef ESC_STATUS_H_
#define ESC_STATUS_H_
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
/**
* @addtogroup topics @{
*/
/**
* The number of ESCs supported.
* Current (Q2/2013) we support 8 ESCs,
*/
#define CONNECTED_ESC_MAX 8
enum ESC_VENDOR {
ESC_VENDOR_GENERIC = 0, /**< generic ESC */
ESC_VENDOR_MIKROKOPTER /**< Mikrokopter */
};
enum ESC_CONNECTION_TYPE {
ESC_CONNECTION_TYPE_PPM = 0, /**< Traditional PPM ESC */
ESC_CONNECTION_TYPE_SERIAL, /**< Serial Bus connected ESC */
ESC_CONNECTION_TYPE_ONESHOOT, /**< One Shoot PPM */
ESC_CONNECTION_TYPE_I2C, /**< I2C */
ESC_CONNECTION_TYPE_CAN /**< CAN-Bus */
};
/**
*
*/
struct esc_status_s
{
/* use of a counter and timestamp recommended (but not necessary) */
uint16_t counter; /**< incremented by the writing thread everytime new data is stored */
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
uint8_t esc_count; /**< number of connected ESCs */
enum ESC_CONNECTION_TYPE esc_connectiontype; /**< how ESCs connected to the system */
struct {
uint16_t esc_address; /**< Address of current ESC (in most cases 1-8 / must be set by driver) */
enum ESC_VENDOR esc_vendor; /**< Vendor of current ESC */
uint16_t esc_version; /**< Version of current ESC - if supported */
uint16_t esc_voltage; /**< Voltage measured from current ESC - if supported */
uint16_t esc_current; /**< Current measured from current ESC (100mA steps) - if supported */
uint16_t esc_rpm; /**< RPM measured from current ESC - if supported */
uint16_t esc_temperature; /**< Temperature measured from current ESC - if supported */
float esc_setpoint; /**< setpoint of current ESC */
uint16_t esc_setpoint_raw; /**< setpoint of current ESC (Value sent to ESC) */
uint16_t esc_state; /**< State of ESC - depend on Vendor */
uint16_t esc_errorcount; /**< Number of reported errors by ESC - if supported */
} esc[CONNECTED_ESC_MAX];
};
/**
* @}
*/
/* register this as object request broker structure */
//ORB_DECLARE(esc_status);
ORB_DECLARE_OPTIONAL(esc_status);
#endif