fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-13 18:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged master

Browse Source
This commit is contained in:
Lorenz Meier
2013-07-15 15:02:45 +02:00
parent 88389ea255 17338ca61a
commit bf2ff98856
56 changed files with 1926 additions and 958 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+6 -6
View File
@@ -661,10 +661,10 @@ int KalmanNav::correctPos()
Vector y(6);
y(0) = _gps.vel_n_m_s - vN;
y(1) = _gps.vel_e_m_s - vE;
y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG;
y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG;
y(4) = double(_gps.alt) / 1.0e3 - alt;
y(5) = double(_sensors.baro_alt_meter) - alt;
y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG;
y(3) = double(_gps.lon) - double(lon) * 1.0e7 * M_RAD_TO_DEG;
y(4) = _gps.alt / 1.0e3f - alt;
y(5) = _sensors.baro_alt_meter - alt;
// compute correction
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
@@ -698,7 +698,7 @@ int KalmanNav::correctPos()
vD += xCorrect(VD);
lat += double(xCorrect(LAT));
lon += double(xCorrect(LON));
alt += double(xCorrect(ALT));
alt += xCorrect(ALT);
// update state covariance
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
@@ -710,7 +710,7 @@ int KalmanNav::correctPos()
static int counter = 0;
if (beta > _faultPos.get() && (counter % 10 == 0)) {
warnx("fault in gps: beta = %8.4f", (double)beta);
warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f",
warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f, baro: %8.4f",
double(y(0) / sqrtf(RPos(0, 0))),
double(y(1) / sqrtf(RPos(1, 1))),
double(y(2) / sqrtf(RPos(2, 2))),
src/modules/commander/commander.c
+14 -4
View File
@@ -917,6 +917,7 @@ int commander_thread_main(int argc, char *argv[])
/* XXX use this! */
//uint64_t failsave_ll_start_time = 0;
enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode;
bool state_changed = true;
bool param_init_forced = true;
@@ -1567,17 +1568,23 @@ int commander_thread_main(int argc, char *argv[])
*/
// printf("checking\n");
if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
(current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
) {
arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd);
tune_positive();
if (current_status.mode_switch != MODE_SWITCH_MANUAL) {
mavlink_log_critical(mavlink_fd, "DISARM DENY, go manual mode first");
tune_negative();
} else {
arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd);
tune_positive();
}
} else {
mavlink_log_critical(mavlink_fd, "STICK DISARM not allowed");
mavlink_log_critical(mavlink_fd, "DISARM not allowed");
tune_negative();
}
stick_off_counter = 0;
@@ -1589,7 +1596,10 @@ int commander_thread_main(int argc, char *argv[])
}
/* check if left stick is in lower right position --> arm */
if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) {
if (current_status.flag_control_manual_enabled &&
current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS &&
sp_man.yaw > STICK_ON_OFF_LIMIT &&
sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
arming_state_transition(status_pub, &current_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd);
stick_on_counter = 0;
src/modules/gpio_led/gpio_led.c
+85 -22
View File
@@ -54,10 +54,12 @@
#include <uORB/topics/actuator_safety.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;
@@ -78,51 +80,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]);
@@ -134,15 +182,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 */
@@ -153,11 +212,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)
@@ -207,7 +266,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);
}
@@ -218,7 +276,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>
@@ -464,7 +463,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));
}
@@ -472,7 +471,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];
@@ -481,7 +480,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
+2 -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"
@@ -372,7 +373,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt);
} else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, global_pos->lat, global_pos->lon, global_pos->relative_alt);
dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt);
} else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) {
dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z);
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
@@ -109,6 +109,7 @@
#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_SAFETY_OFF (1 << 12) /* safety is off */
#define PX4IO_P_STATUS_FLAGS_RC_DSM11 (1 << 13) /* 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 */
@@ -161,7 +162,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
@@ -187,6 +187,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
@@ -446,10 +446,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:
@@ -457,6 +457,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
+64 -3
View File
@@ -73,6 +73,7 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_control_debug.h>
@@ -81,6 +82,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,9 +616,9 @@ int sdlog2_thread_main(int argc, char *argv[])
errx(1, "can't allocate log buffer, exiting.");
}
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* number of messages */
const ssize_t fdsc = 20;
/* --- IMPORTANT: DEFINE MAX NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* max number of messages */
const ssize_t fdsc = 21;
/* Sanity check variable and index */
ssize_t fdsc_count = 0;
@@ -642,6 +644,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct vehicle_local_position_s local_pos;
struct vehicle_local_position_setpoint_s local_pos_sp;
struct vehicle_global_position_s global_pos;
struct vehicle_global_position_setpoint_s global_pos_sp;
struct vehicle_gps_position_s gps_pos;
struct vehicle_vicon_position_s vicon_pos;
struct vehicle_control_debug_s control_debug;
@@ -649,6 +652,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));
@@ -666,12 +670,14 @@ int sdlog2_thread_main(int argc, char *argv[])
int local_pos_sub;
int local_pos_sp_sub;
int global_pos_sub;
int global_pos_sp_sub;
int gps_pos_sub;
int vicon_pos_sub;
int control_debug_sub;
int flow_sub;
int rc_sub;
int airspeed_sub;
int esc_sub;
} subs;
/* log message buffer: header + body */
@@ -696,6 +702,8 @@ 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_GPSP_s log_GPSP;
struct log_ESC_s log_ESC;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@@ -787,6 +795,12 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- GLOBAL POSITION SETPOINT--- */
subs.global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
fds[fdsc_count].fd = subs.global_pos_sp_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- VICON POSITION --- */
subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
fds[fdsc_count].fd = subs.vicon_pos_sub;
@@ -817,6 +831,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.
@@ -1108,6 +1128,25 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(GPOS);
}
/* --- GLOBAL POSITION SETPOINT --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_global_position_setpoint), subs.global_pos_sp_sub, &buf.global_pos_sp);
log_msg.msg_type = LOG_GPSP_MSG;
log_msg.body.log_GPSP.altitude_is_relative = buf.global_pos_sp.altitude_is_relative;
log_msg.body.log_GPSP.lat = buf.global_pos_sp.lat;
log_msg.body.log_GPSP.lon = buf.global_pos_sp.lon;
log_msg.body.log_GPSP.altitude = buf.global_pos_sp.altitude;
log_msg.body.log_GPSP.yaw = buf.global_pos_sp.yaw;
log_msg.body.log_GPSP.loiter_radius = buf.global_pos_sp.loiter_radius;
log_msg.body.log_GPSP.loiter_direction = buf.global_pos_sp.loiter_direction;
log_msg.body.log_GPSP.nav_cmd = buf.global_pos_sp.nav_cmd;
log_msg.body.log_GPSP.param1 = buf.global_pos_sp.param1;
log_msg.body.log_GPSP.param2 = buf.global_pos_sp.param2;
log_msg.body.log_GPSP.param3 = buf.global_pos_sp.param3;
log_msg.body.log_GPSP.param4 = buf.global_pos_sp.param4;
LOGBUFFER_WRITE_AND_COUNT(GPSP);
}
/* --- VICON POSITION --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos);
@@ -1167,6 +1206,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
+43 -6
View File
@@ -152,15 +152,15 @@ struct log_ATTC_s {
/* --- STAT - VEHICLE STATE --- */
#define LOG_STAT_MSG 10
struct log_STAT_s {
unsigned char state;
unsigned char flight_mode;
unsigned char manual_control_mode;
unsigned char manual_sas_mode;
unsigned char armed;
uint8_t state;
uint8_t flight_mode;
uint8_t manual_control_mode;
uint8_t manual_sas_mode;
uint8_t armed;
float battery_voltage;
float battery_current;
float battery_remaining;
unsigned char battery_warning;
uint8_t battery_warning;
};
/* --- CTRL - CONTROL DEBUG --- */
@@ -229,6 +229,41 @@ struct log_GPOS_s {
float vel_e;
float vel_d;
};
/* --- GPSP - GLOBAL POSITION SETPOINT --- */
#define LOG_GPSP_MSG 17
struct log_GPSP_s {
uint8_t altitude_is_relative;
int32_t lat;
int32_t lon;
float altitude;
float yaw;
float loiter_radius;
int8_t loiter_direction;
uint8_t nav_cmd;
float param1;
float param2;
float param3;
float param4;
};
/* --- ESC - ESC STATE --- */
#define LOG_ESC_MSG 18
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 */
@@ -251,6 +286,8 @@ 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(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
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
@@ -228,6 +228,8 @@ private:
float rc_scale_flaps;
float battery_voltage_scaling;
int rc_rl1_DSM_VCC_control;
} _parameters; /**< local copies of interesting parameters */
struct {
@@ -274,6 +276,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 */
@@ -509,6 +513,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();
}
@@ -715,6 +722,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/airspeed.c
+2 -2
View File
@@ -62,7 +62,7 @@ float calc_indicated_airspeed(float differential_pressure)
if (differential_pressure > 0) {
return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
} else {
return -sqrtf((2.0f*fabs(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
return -sqrtf((2.0f*fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
}
}
@@ -106,6 +106,6 @@ float calc_true_airspeed(float total_pressure, float static_pressure, float temp
return sqrtf((2.0f*(pressure_difference)) / density);
} else
{
return -sqrtf((2.0f*fabs(pressure_difference)) / density);
return -sqrtf((2.0f*fabsf(pressure_difference)) / density);
}
}
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
@@ -177,3 +177,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
+115
View File
@@ -0,0 +1,115 @@
/****************************************************************************
*
* 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 */
ESC_VENDOR_GRAUPNER_HOTT /**< Graupner HoTT ESC */
};
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
src/modules/uORB/topics/vehicle_global_position_setpoint.h
+1 -1
View File
@@ -66,7 +66,7 @@ struct vehicle_global_position_setpoint_s
float altitude; /**< altitude in meters */
float yaw; /**< in radians NED -PI..+PI */
float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */
uint8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */
int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */
enum NAV_CMD nav_cmd; /**< true if loitering is enabled */
float param1;
float param2;