diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-07-15 15:02:45 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-07-15 15:02:45 +0200 |
commit | bf2ff98856b7e6b107a7ec5bbde3b00e38713804 (patch) | |
tree | ad11c218b7524826e45c58257cdaac12e8a1ac53 /src/modules | |
parent | 88389ea2554c6f56a4fdd86cdd86a1e7b6affc21 (diff) | |
parent | 17338ca61aa8a58c92ae621de94240ddd22f28a2 (diff) | |
download | px4-firmware-bf2ff98856b7e6b107a7ec5bbde3b00e38713804.tar.gz px4-firmware-bf2ff98856b7e6b107a7ec5bbde3b00e38713804.tar.bz2 px4-firmware-bf2ff98856b7e6b107a7ec5bbde3b00e38713804.zip |
Merged master
Diffstat (limited to 'src/modules')
29 files changed, 459 insertions, 93 deletions
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 97d7fdd75..191d20f30 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -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))), diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 9d52d4f1c..2b0b3a415 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -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, ¤t_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, ¤t_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, ¤t_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd); stick_on_counter = 0; diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 618095d0b..97b585cb7 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -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); + } } diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index f6c371c7c..a2758a45c 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -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]; diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index 0010bb341..149efda60 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -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 */ diff --git a/src/modules/mavlink/mavlink_parameters.c b/src/modules/mavlink/mavlink_parameters.c index 819f3441b..18ca7a854 100644 --- a/src/modules/mavlink/mavlink_parameters.c +++ b/src/modules/mavlink/mavlink_parameters.c @@ -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 */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 33ac14860..01bbabd46 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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; diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c index 4b010dd59..be88b8794 100644 --- a/src/modules/mavlink/missionlib.c +++ b/src/modules/mavlink/missionlib.c @@ -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> diff --git a/src/modules/mavlink/missionlib.h b/src/modules/mavlink/missionlib.h index c2ca735b3..c7d8f90c5 100644 --- a/src/modules/mavlink/missionlib.h +++ b/src/modules/mavlink/missionlib.h @@ -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); diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index a9a611998..95bc8fdc0 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -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> diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index cefcca468..eea928a17 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -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); diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h index c32ab32e5..96a0ecd30 100644 --- a/src/modules/mavlink/waypoints.h +++ b/src/modules/mavlink/waypoints.h @@ -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> diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index 35d53b880..2d5a64ee8 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -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> diff --git a/src/modules/mavlink_onboard/mavlink_bridge_header.h b/src/modules/mavlink_onboard/mavlink_bridge_header.h index 3ad3bb617..b72bbb2b1 100644 --- a/src/modules/mavlink_onboard/mavlink_bridge_header.h +++ b/src/modules/mavlink_onboard/mavlink_bridge_header.h @@ -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 */ diff --git a/src/modules/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c index 2d406fb9f..0236e6126 100644 --- a/src/modules/mavlink_onboard/mavlink_receiver.c +++ b/src/modules/mavlink_onboard/mavlink_receiver.c @@ -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> diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 3cf9ca149..43d96fb06 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -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); diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index ea35e5513..ab6e3fec4 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -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. */ diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 0e477a200..ca58ba0eb 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -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 */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 042e7fe66..a020f8704 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -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); diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index bd13f3b7d..bf895d31f 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -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; } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 31da81f5c..80ee3adee 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -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 diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 358637f93..9be96a62e 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -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); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 7077972aa..3ed9efc8b 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -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)); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 49a1da83b..acc0c2717 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -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; } diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c index 15bb833a9..e01cc4dda 100644 --- a/src/modules/systemlib/airspeed.c +++ b/src/modules/systemlib/airspeed.c @@ -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); } } diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c index 8fdff8ac0..afc5b072c 100644 --- a/src/modules/systemlib/cpuload.c +++ b/src/modules/systemlib/cpuload.c @@ -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++; - - /* 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++; - // } + int static_tasks_count = 2; // there are at least 2 threads that should be initialized statically - "idle" and "init" + +#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) diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 8ae47ba8b..a061fe676 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -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); diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h new file mode 100644 index 000000000..00cf59b28 --- /dev/null +++ b/src/modules/uORB/topics/esc_status.h @@ -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 diff --git a/src/modules/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/vehicle_global_position_setpoint.h index 3ae3ff28c..5c8ce1e4d 100644 --- a/src/modules/uORB/topics/vehicle_global_position_setpoint.h +++ b/src/modules/uORB/topics/vehicle_global_position_setpoint.h @@ -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; |