diff options
author | Julian Oes <joes@student.ethz.ch> | 2012-11-05 16:22:21 -0800 |
---|---|---|
committer | Julian Oes <joes@student.ethz.ch> | 2012-11-05 16:22:21 -0800 |
commit | 339882063158ce99c628fa75fd9325c377f63f16 (patch) | |
tree | 680020961e99228c379f02067402f46573551b0a | |
parent | 7ee9628559c57d876f57dfb936e13647c0604cd0 (diff) | |
parent | 7d76a8a57b41c82db0712dd0544e67d1ce97d89c (diff) | |
download | px4-firmware-339882063158ce99c628fa75fd9325c377f63f16.tar.gz px4-firmware-339882063158ce99c628fa75fd9325c377f63f16.tar.bz2 px4-firmware-339882063158ce99c628fa75fd9325c377f63f16.zip |
Merge branch 'master' of https://github.com/PX4/Firmware
-rw-r--r-- | apps/drivers/drv_rc_input.h | 22 | ||||
-rw-r--r-- | apps/drivers/px4io/px4io.cpp | 88 | ||||
-rw-r--r-- | apps/drivers/stm32/drv_hrt.c | 2 | ||||
-rw-r--r-- | apps/mavlink/mavlink.c | 15 | ||||
-rw-r--r-- | apps/mavlink/mavlink_receiver.c | 4 | ||||
-rw-r--r-- | apps/mavlink/orb_listener.c | 35 | ||||
-rw-r--r-- | apps/mavlink/orb_topics.h | 2 | ||||
-rw-r--r-- | apps/mavlink/waypoints.c | 7 | ||||
-rw-r--r-- | apps/multirotor_pos_control/multirotor_pos_control.c | 6 | ||||
-rw-r--r-- | apps/px4io/comms.c | 51 | ||||
-rw-r--r-- | apps/px4io/mixer.c | 199 | ||||
-rw-r--r-- | apps/px4io/protocol.h | 12 | ||||
-rw-r--r-- | apps/px4io/px4io.c | 14 | ||||
-rw-r--r-- | apps/px4io/px4io.h | 11 | ||||
-rw-r--r-- | apps/sensors/sensors.cpp | 198 | ||||
-rw-r--r-- | apps/systemlib/geo/geo.c | 144 | ||||
-rw-r--r-- | apps/systemlib/geo/geo.h | 8 | ||||
-rw-r--r-- | apps/systemlib/pid/pid.c | 4 | ||||
-rw-r--r-- | apps/uORB/topics/rc_channels.h | 14 | ||||
-rwxr-xr-x | nuttx/configs/px4io/io/defconfig | 6 |
20 files changed, 663 insertions, 179 deletions
diff --git a/apps/drivers/drv_rc_input.h b/apps/drivers/drv_rc_input.h index 532e95fb5..927406108 100644 --- a/apps/drivers/drv_rc_input.h +++ b/apps/drivers/drv_rc_input.h @@ -57,15 +57,23 @@ #define RC_INPUT_DEVICE_PATH "/dev/input_rc" /** - * Maximum number of R/C input channels in the system. + * Maximum number of R/C input channels in the system. S.Bus has up to 18 channels. */ -#define RC_INPUT_MAX_CHANNELS 16 +#define RC_INPUT_MAX_CHANNELS 18 /** * Input signal type, value is a control position from zero to 100 * percent. */ -typedef uint8_t rc_input_t; +typedef uint16_t rc_input_t; + +enum RC_INPUT_SOURCE { + RC_INPUT_SOURCE_UNKNOWN = 0, + RC_INPUT_SOURCE_PX4FMU_PPM, + RC_INPUT_SOURCE_PX4IO_PPM, + RC_INPUT_SOURCE_PX4IO_SPEKTRUM, + RC_INPUT_SOURCE_PX4IO_SBUS +}; /** * R/C input status structure. @@ -74,10 +82,16 @@ typedef uint8_t rc_input_t; * on the board involved. */ struct rc_input_values { + /** decoding time */ + uint64_t timestamp; + /** number of channels actually being seen */ uint32_t channel_count; - /** desired pulse widths for each of the supported channels */ + /** Input source */ + enum RC_INPUT_SOURCE input_source; + + /** measured pulse widths for each of the supported channels */ rc_input_t values[RC_INPUT_MAX_CHANNELS]; }; diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 5050c6ce7..4687df2aa 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -61,9 +61,10 @@ #include <drivers/device/device.h> #include <drivers/drv_rc_input.h> #include <drivers/drv_pwm_output.h> -#include <systemlib/mixer/mixer.h> +#include <drivers/drv_hrt.h> #include <drivers/drv_mixer.h> +#include <systemlib/mixer/mixer.h> #include <systemlib/perf_counter.h> #include <systemlib/hx_stream.h> #include <systemlib/err.h> @@ -87,6 +88,8 @@ public: virtual int ioctl(file *filp, int cmd, unsigned long arg); + void set_rx_mode(unsigned mode); + private: static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS; @@ -103,6 +106,9 @@ private: int _t_armed; ///< system armed control topic actuator_armed_s _armed; ///< system armed state + orb_advert_t _to_input_rc; ///< rc inputs from io + rc_input_values _input_rc; ///< rc input values + orb_advert_t _t_outputs; ///< mixed outputs topic actuator_outputs_s _outputs; ///< mixed outputs @@ -114,6 +120,9 @@ private: // XXX how should this work? bool _send_needed; ///< If true, we need to send a packet to IO + bool _config_needed; ///< if true, we need to set a config update to IO + + uint8_t _rx_mode; ///< the current RX mode on IO /** * Trampoline to the worker task @@ -146,6 +155,11 @@ private: void io_send(); /** + * Send a config packet to PX4IO + */ + void config_send(); + + /** * Mixer control callback; invoked to fetch a control from a specific * group/index during mixing. */ @@ -176,7 +190,9 @@ PX4IO::PX4IO() : _mixers(nullptr), _primary_pwm_device(false), _switch_armed(false), - _send_needed(false) + _send_needed(false), + _config_needed(false), + _rx_mode(RX_MODE_PPM_ONLY) { /* we need this potentially before it could be set in task_main */ g_dev = this; @@ -305,9 +321,14 @@ PX4IO::task_main() orb_set_interval(_t_armed, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ + memset(&_outputs, 0, sizeof(_outputs)); _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &_outputs); + /* advertise the rc inputs */ + memset(&_input_rc, 0, sizeof(_input_rc)); + _to_input_rc = orb_advertise(ORB_ID(input_rc), &_input_rc); + /* poll descriptor */ pollfd fds[3]; fds[0].fd = _serial_fd; @@ -373,6 +394,12 @@ PX4IO::task_main() _send_needed = false; io_send(); } + + /* send a config packet to IO if required */ + if (_config_needed) { + _config_needed = false; + config_send(); + } } out: @@ -438,7 +465,14 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received) } _connected = true; - /* XXX handle R/C inputs here ... needs code sharing/library */ + /* publish raw rc channel values from IO */ + _input_rc.timestamp = hrt_absolute_time(); + for (int i = 0; i < rep->channel_count; i++) + { + _input_rc.values[i] = rep->rc_channel[i]; + } + + orb_publish(ORB_ID(input_rc), _to_input_rc, &_input_rc); /* remember the latched arming switch state */ _switch_armed = rep->armed; @@ -473,6 +507,20 @@ PX4IO::io_send() debug("send error %d", ret); } +void +PX4IO::config_send() +{ + px4io_config cfg; + int ret; + + cfg.f2i_config_magic = F2I_CONFIG_MAGIC; + cfg.serial_rx_mode = _rx_mode; + + ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg)); + if (ret) + debug("config error %d", ret); +} + int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) { @@ -586,6 +634,15 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) return ret; } +void +PX4IO::set_rx_mode(unsigned mode) +{ + if (mode != _rx_mode) { + _rx_mode = mode; + _config_needed = true; + } +} + extern "C" __EXPORT int px4io_main(int argc, char *argv[]); namespace @@ -642,9 +699,6 @@ px4io_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "test")) - test(); - /* note, stop not currently implemented */ if (!strcmp(argv[1], "update")) { @@ -690,5 +744,25 @@ px4io_main(int argc, char *argv[]) return ret; } - errx(1, "need a verb, only support 'start' and 'update'"); + if (!strcmp(argv[1], "rx_spektrum6")) { + if (g_dev == nullptr) + errx(1, "not started"); + g_dev->set_rx_mode(RX_MODE_SPEKTRUM_6); + } + if (!strcmp(argv[1], "rx_spektrum7")) { + if (g_dev == nullptr) + errx(1, "not started"); + g_dev->set_rx_mode(RX_MODE_SPEKTRUM_7); + } + if (!strcmp(argv[1], "rx_sbus")) { + if (g_dev == nullptr) + errx(1, "not started"); + g_dev->set_rx_mode(RX_MODE_FUTABA_SBUS); + } + + if (!strcmp(argv[1], "test")) + test(); + + + errx(1, "need a command, try 'start', 'test', 'rx_spektrum6', 'rx_spektrum7', 'rx_sbus' or 'update'"); } diff --git a/apps/drivers/stm32/drv_hrt.c b/apps/drivers/stm32/drv_hrt.c index 6ac46092b..cd9cb45b1 100644 --- a/apps/drivers/stm32/drv_hrt.c +++ b/apps/drivers/stm32/drv_hrt.c @@ -338,7 +338,7 @@ static void hrt_call_invoke(void); /* decoded PPM buffer */ #define PPM_MAX_CHANNELS 12 __EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS]; -__EXPORT unsigned ppm_decoded_channels; +__EXPORT unsigned ppm_decoded_channels = 0; __EXPORT uint64_t ppm_last_valid_decode = 0; /* PPM edge history */ diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 7d8232b3a..9b2cfcbba 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -589,16 +589,15 @@ int mavlink_thread_main(int argc, char *argv[]) /* 2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); } else if (baudrate >= 115200) { - /* 50 Hz / 20 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 200); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 200); /* 20 Hz / 50 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); - /* 10 Hz / 100 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); + /* 5 Hz / 100 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); - /* 1 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 1000); + /* 2 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); } else if (baudrate >= 57600) { /* 10 Hz / 100 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300); diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 0dff743bc..39e574c04 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -397,10 +397,6 @@ handle_message(mavlink_message_t *msg) rc_hil.timestamp = hrt_absolute_time(); rc_hil.chan_count = 4; - rc_hil.chan[0].raw = 1500 + man.x / 2; - rc_hil.chan[1].raw = 1500 + man.y / 2; - rc_hil.chan[2].raw = 1500 + man.r / 2; - rc_hil.chan[3].raw = 1500 + man.z / 2; rc_hil.chan[0].scaled = man.x / 1000.0f; rc_hil.chan[1].scaled = man.y / 1000.0f; diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 4b66ee438..683964a0d 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -67,6 +67,7 @@ struct vehicle_global_position_s global_pos; struct vehicle_local_position_s local_pos; struct vehicle_status_s v_status; struct rc_channels_s rc; +struct rc_input_values rc_raw; struct actuator_armed_s armed; struct mavlink_subscriptions mavlink_subs; @@ -99,6 +100,7 @@ static void l_vehicle_attitude(struct listener *l); static void l_vehicle_gps_position(struct listener *l); static void l_vehicle_status(struct listener *l); static void l_rc_channels(struct listener *l); +static void l_input_rc(struct listener *l); static void l_global_position(struct listener *l); static void l_local_position(struct listener *l); static void l_global_position_setpoint(struct listener *l); @@ -116,6 +118,7 @@ struct listener listeners[] = { {l_vehicle_gps_position, &mavlink_subs.gps_sub, 0}, {l_vehicle_status, &status_sub, 0}, {l_rc_channels, &rc_sub, 0}, + {l_input_rc, &mavlink_subs.input_rc_sub, 0}, {l_global_position, &mavlink_subs.global_pos_sub, 0}, {l_local_position, &mavlink_subs.local_pos_sub, 0}, {l_global_position_setpoint, &mavlink_subs.spg_sub, 0}, @@ -274,21 +277,29 @@ l_rc_channels(struct listener *l) { /* copy rc channels into local buffer */ orb_copy(ORB_ID(rc_channels), rc_sub, &rc); + // XXX Add RC channels scaled message here +} + +void +l_input_rc(struct listener *l) +{ + /* copy rc channels into local buffer */ + orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw); if (gcs_link) /* Channels are sent in MAVLink main loop at a fixed interval */ mavlink_msg_rc_channels_raw_send(chan, - rc.timestamp / 1000, + rc_raw.timestamp / 1000, 0, - rc.chan[0].raw, - rc.chan[1].raw, - rc.chan[2].raw, - rc.chan[3].raw, - rc.chan[4].raw, - rc.chan[5].raw, - rc.chan[6].raw, - rc.chan[7].raw, - rc.rssi); + (rc_raw.channel_count > 0) ? rc_raw.values[0] : UINT16_MAX, + (rc_raw.channel_count > 1) ? rc_raw.values[1] : UINT16_MAX, + (rc_raw.channel_count > 2) ? rc_raw.values[2] : UINT16_MAX, + (rc_raw.channel_count > 3) ? rc_raw.values[3] : UINT16_MAX, + (rc_raw.channel_count > 4) ? rc_raw.values[4] : UINT16_MAX, + (rc_raw.channel_count > 5) ? rc_raw.values[5] : UINT16_MAX, + (rc_raw.channel_count > 6) ? rc_raw.values[6] : UINT16_MAX, + (rc_raw.channel_count > 7) ? rc_raw.values[7] : UINT16_MAX, + 255); } void @@ -584,6 +595,10 @@ uorb_receive_start(void) rc_sub = orb_subscribe(ORB_ID(rc_channels)); orb_set_interval(rc_sub, 100); /* 10Hz updates */ + /* --- RC RAW VALUE --- */ + mavlink_subs.input_rc_sub = orb_subscribe(ORB_ID(input_rc)); + orb_set_interval(mavlink_subs.input_rc_sub, 100); + /* --- GLOBAL POS VALUE --- */ mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); orb_set_interval(mavlink_subs.global_pos_sub, 1000); /* 1Hz active updates */ diff --git a/apps/mavlink/orb_topics.h b/apps/mavlink/orb_topics.h index f041e7c4c..6860702d2 100644 --- a/apps/mavlink/orb_topics.h +++ b/apps/mavlink/orb_topics.h @@ -57,6 +57,7 @@ #include <uORB/topics/actuator_controls.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/debug_key_value.h> +#include <drivers/drv_rc_input.h> struct mavlink_subscriptions { int sensor_sub; @@ -75,6 +76,7 @@ struct mavlink_subscriptions { int spl_sub; int spg_sub; int debug_key_value; + int input_rc_sub; }; extern struct mavlink_subscriptions mavlink_subs; diff --git a/apps/mavlink/waypoints.c b/apps/mavlink/waypoints.c index 3d9e5750a..16759237e 100644 --- a/apps/mavlink/waypoints.c +++ b/apps/mavlink/waypoints.c @@ -360,7 +360,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ float dist = -1.0f; if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { - dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, global_pos->lat, global_pos->lon, global_pos->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->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); @@ -376,14 +376,13 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ if (dist >= 0.f && dist <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw wpm->pos_reached = true; - if (counter % 10 == 0) + if (counter % 100 == 0) printf("Setpoint reached: %0.4f, orbit: %.4f\n", dist, orbit); } - // else // { // if(counter % 100 == 0) -// printf("Setpoint not reached yet: %0.4f, orbit: %.4f\n",dist,orbit); +// printf("Setpoint not reached yet: %0.4f, orbit: %.4f, coordinate frame: %d\n",dist, orbit, coordinate_frame); // } } diff --git a/apps/multirotor_pos_control/multirotor_pos_control.c b/apps/multirotor_pos_control/multirotor_pos_control.c index 474ced731..9821fc7e5 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control.c +++ b/apps/multirotor_pos_control/multirotor_pos_control.c @@ -196,12 +196,14 @@ multirotor_pos_control_thread_main(int argc, char *argv[]) float x_setpoint = 0.0f; + // XXX enable switching between Vicon and local position estimate /* local pos is the Vicon position */ - att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p * dT; + // XXX just an example, lacks rotation around world-body transformation + att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p; att_sp.roll_body = 0.0f; att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.4f; + att_sp.thrust = 0.3f; att_sp.timestamp = hrt_absolute_time(); /* publish new attitude setpoint */ diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index 8fdf7d1c8..1edff25b1 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -116,18 +116,32 @@ int frame_rx; int frame_bad; static void -comms_handle_frame(void *arg, const void *buffer, size_t length) +comms_handle_config(const void *buffer, size_t length) { - struct px4io_command *cmd = (struct px4io_command *)buffer; + const struct px4io_config *cfg = (struct px4io_config *)buffer; - /* make sure it's what we are expecting */ - if ((length != sizeof(struct px4io_command)) || - (cmd->f2i_magic != F2I_MAGIC)) { - frame_bad++; + if (length != sizeof(*cfg)) { + frame_bad++; return; } + frame_rx++; + mixer_set_serial_mode(cfg->serial_rx_mode); + +} + +static void +comms_handle_command(const void *buffer, size_t length) +{ + const struct px4io_command *cmd = (struct px4io_command *)buffer; + + if (length != sizeof(*cmd)) { + frame_bad++; + return; + } + + frame_rx++; irqstate_t flags = irqsave(); /* fetch new PWM output values */ @@ -146,6 +160,29 @@ comms_handle_frame(void *arg, const void *buffer, size_t length) for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) system_state.relays[i] = cmd->relay_state[i]; -out: irqrestore(flags); } + + +static void +comms_handle_frame(void *arg, const void *buffer, size_t length) +{ + const uint16_t *type = (const uint16_t *)buffer; + + + /* make sure it's what we are expecting */ + if (length > 2) { + switch (*type) { + case F2I_MAGIC: + comms_handle_command(buffer, length); + break; + case F2I_CONFIG_MAGIC: + comms_handle_config(buffer, length); + break; + default: + break; + } + } + frame_bad++; +} + diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index fab577383..471965fd7 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -40,10 +40,13 @@ #include <sys/types.h> #include <stdbool.h> - +#include <string.h> #include <assert.h> #include <errno.h> #include <fcntl.h> +#include <termios.h> +#include <unistd.h> +#include <fcntl.h> #include <drivers/drv_pwm_output.h> #include <drivers/drv_hrt.h> @@ -65,6 +68,11 @@ static unsigned fmu_input_drops; #define FMU_INPUT_DROP_LIMIT 20 /* + * Serial port fd for serial RX protocols + */ +static int rx_port = -1; + +/* * HRT periodic call used to check for control input data. */ static struct hrt_call mixer_input_call; @@ -98,12 +106,47 @@ struct mixer { int mixer_init(void) { + /* open the serial port */ + rx_port = open("/dev/ttyS0", O_RDONLY | O_NONBLOCK); + /* look for control data at 50Hz */ hrt_call_every(&mixer_input_call, 1000, 20000, mixer_tick, NULL); return 0; } +void +mixer_set_serial_mode(uint8_t serial_mode) +{ + + if (serial_mode == system_state.serial_rx_mode) + return; + + struct termios t; + tcgetattr(rx_port, &t); + + switch (serial_mode) { + case RX_MODE_PPM_ONLY: + break; + case RX_MODE_SPEKTRUM_6: + case RX_MODE_SPEKTRUM_7: + /* 115200, no parity, one stop bit */ + cfsetspeed(&t, 115200); + t.c_cflag &= ~(CSTOPB | PARENB); + break; + case RX_MODE_FUTABA_SBUS: + /* 100000, even parity, two stop bits */ + cfsetspeed(&t, 100000); + t.c_cflag |= (CSTOPB | PARENB); + break; + default: + return; + } + + tcsetattr(rx_port, TCSANOW, &t); + system_state.serial_rx_mode = serial_mode; +} + static void mixer_tick(void *arg) { @@ -181,22 +224,145 @@ mixer_tick(void *arg) } static void -mixer_get_rc_input(void) +mixer_update(int mixer, uint16_t *inputs, int input_count) { + /* simple passthrough for now */ + if (mixer < input_count) { + mixers[mixer].current_value = inputs[mixer]; + } else { + mixers[mixer].current_value = 0; + } +} + +static bool +mixer_get_spektrum_input(void) +{ + static uint8_t buf[16]; + static unsigned count; + + /* always read as much data as we can into the buffer */ + if (count >= sizeof(buf)) + count = 0; + ssize_t result = read(rx_port, buf, sizeof(buf) - count); + /* no data or an error */ + if (result <= 0) + return false; + count += result; + + /* if there are more than two bytes in the buffer, check for sync */ + if (count >= 2) { + if ((buf[0] != 0x3) || (buf[1] != 0x1)) { + /* not in sync; look for a possible sync marker */ + for (unsigned i = 1; i < count; i++) { + if (buf[i] == 0x3) { + /* could be a frame marker; move buffer bytes */ + count -= i; + memmove(buf, buf + i, count); + break; + } + } + } + } + if (count < sizeof(buf)) + return false; + + /* we got a frame; decode it */ + const uint16_t *channels = (const uint16_t *)&buf[2]; + /* - * XXX currently only supporting PPM + * Channel assignment for DX6i vs. DX7 is different. + * + * DX7 etc. is: + * + * 0: Aileron + * 1: Flaps + * 2: Gear + * 3: Elevator + * 4: Aux2 + * 5: Throttle + * 6: Rudder + * + * DX6i is: * - * XXX check timestamp to ensure current + * 0: Aileron + * 1: Flaps + * 2: Elevator + * 3: Rudder + * 4: Throttle + * 5: Gear + * 6: <notused> + * + * We convert these to our standard Futaba-style assignment: + * + * 0: Throttle (Throttle) + * 1: Roll (Aileron) + * 2: Pitch (Elevator) + * 3: Yaw (Rudder) + * 4: Override (Flaps) + * 5: FUNC_0 (Gear) + * 6: FUNC_1 (Aux2) */ - if (ppm_decoded_channels > 0) { - mixer_input_drops = 0; - system_state.fmu_report_due = true; + if (system_state.serial_rx_mode == RX_MODE_SPEKTRUM_7) { + system_state.rc_channel_data[0] = channels[5]; /* Throttle */ + system_state.rc_channel_data[1] = channels[0]; /* Roll */ + system_state.rc_channel_data[2] = channels[3]; /* Pitch */ + system_state.rc_channel_data[3] = channels[6]; /* Yaw */ + system_state.rc_channel_data[4] = channels[1]; /* Override */ + system_state.rc_channel_data[5] = channels[2]; /* FUNC_0 */ + system_state.rc_channel_data[6] = channels[4]; /* FUNC_1 */ + system_state.rc_channels = 7; + } else { + system_state.rc_channel_data[0] = channels[4]; /* Throttle */ + system_state.rc_channel_data[1] = channels[0]; /* Roll */ + system_state.rc_channel_data[2] = channels[2]; /* Pitch */ + system_state.rc_channel_data[3] = channels[3]; /* Yaw */ + system_state.rc_channel_data[4] = channels[1]; /* Override */ + system_state.rc_channel_data[5] = channels[5]; /* FUNC_0 */ + system_state.rc_channels = 6; + } + count = 0; + return true; +} + +static bool +mixer_get_sbus_input(void) +{ + /* XXX not implemented yet */ + return false; +} + +static void +mixer_get_rc_input(void) +{ + bool got_input = false; + + switch (system_state.serial_rx_mode) { + case RX_MODE_PPM_ONLY: + if (ppm_decoded_channels > 0) { + /* copy channel data */ + system_state.rc_channels = ppm_decoded_channels; + for (unsigned i = 0; i < ppm_decoded_channels; i++) + system_state.rc_channel_data[i] = ppm_buffer[i]; + got_input = true; + } + break; + + case RX_MODE_SPEKTRUM_6: + case RX_MODE_SPEKTRUM_7: + got_input = mixer_get_spektrum_input(); + break; - /* copy channel data */ - system_state.rc_channels = ppm_decoded_channels; - for (unsigned i = 0; i < ppm_decoded_channels; i++) - system_state.rc_channel_data[i] = ppm_buffer[i]; + case RX_MODE_FUTABA_SBUS: + got_input = mixer_get_sbus_input(); + break; + default: + break; + } + + if (got_input) { + mixer_input_drops = 0; + system_state.fmu_report_due = true; } else { /* * No data; count the 'frame drops' and once we hit the limit @@ -213,14 +379,3 @@ mixer_get_rc_input(void) } } } - -static void -mixer_update(int mixer, uint16_t *inputs, int input_count) -{ - /* simple passthrough for now */ - if (mixer < input_count) { - mixers[mixer].current_value = inputs[mixer]; - } else { - mixers[mixer].current_value = 0; - } -} diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index c4a63d886..7467f1adc 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -62,6 +62,18 @@ struct px4io_command { bool arm_ok; }; +/* config message from FMU to IO */ +struct px4io_config { + uint16_t f2i_config_magic; +#define F2I_CONFIG_MAGIC 0x6366 + + uint8_t serial_rx_mode; +#define RX_MODE_PPM_ONLY 0 +#define RX_MODE_SPEKTRUM_6 1 +#define RX_MODE_SPEKTRUM_7 2 +#define RX_MODE_FUTABA_SBUS 3 +}; + /* report from IO to FMU */ struct px4io_report { uint16_t i2f_magic; diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index dc8e227fc..7039e5d58 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -102,7 +102,7 @@ int user_start(int argc, char *argv[]) safety_init(); /* set up some timers for the main loop */ - timers[TIMER_BLINK_AMBER] = 250; /* heartbeat blink @ 2Hz */ + timers[TIMER_BLINK_BLUE] = 250; /* heartbeat blink @ 2Hz */ timers[TIMER_STATUS_PRINT] = 1000; /* print status message @ 1Hz */ /* @@ -114,21 +114,21 @@ int user_start(int argc, char *argv[]) comms_check(); /* blink the heartbeat LED */ - if (timers[TIMER_BLINK_AMBER] == 0) { - timers[TIMER_BLINK_AMBER] = 250; - LED_AMBER(heartbeat = !heartbeat); + if (timers[TIMER_BLINK_BLUE] == 0) { + timers[TIMER_BLINK_BLUE] = 250; + LED_BLUE(heartbeat = !heartbeat); } /* blink the failsafe LED if we don't have FMU input */ if (!system_state.mixer_use_fmu) { - if (timers[TIMER_BLINK_BLUE] == 0) { - timers[TIMER_BLINK_BLUE] = 125; + if (timers[TIMER_BLINK_AMBER] == 0) { + timers[TIMER_BLINK_AMBER] = 125; failsafe = !failsafe; } } else { failsafe = false; } - LED_BLUE(failsafe); + LED_AMBER(failsafe); /* print some simple status */ if (timers[TIMER_STATUS_PRINT] == 0) { diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 7aef88102..274b27ff3 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -101,6 +101,12 @@ struct sys_state_s * If true, new control data from the FMU has been received. */ bool fmu_data_received; + + /* + * Current serial interface mode, per the serial_rx_mode parameter + * in the config packet. + */ + uint8_t serial_rx_mode; }; extern struct sys_state_s system_state; @@ -123,8 +129,8 @@ extern volatile int timers[TIMER_NUM_TIMERS]; /* * GPIO handling. */ -#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) -#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) +#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) +#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) #define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) #define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) @@ -141,6 +147,7 @@ extern volatile int timers[TIMER_NUM_TIMERS]; * Mixer */ extern int mixer_init(void); +extern void mixer_set_serial_mode(uint8_t newmode); /* * Safety switch/LED. diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 453502b05..3e9f35eaf 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -58,11 +58,13 @@ #include <drivers/drv_gyro.h> #include <drivers/drv_mag.h> #include <drivers/drv_baro.h> +#include <drivers/drv_rc_input.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> #include <systemlib/err.h> #include <systemlib/perf_counter.h> + #include <systemlib/ppm_decode.h> #include <uORB/uORB.h> @@ -143,6 +145,7 @@ private: int _gyro_sub; /**< raw gyro data subscription */ int _accel_sub; /**< raw accel data subscription */ int _mag_sub; /**< raw mag data subscription */ + int _rc_sub; /**< raw rc channels data subscription */ int _baro_sub; /**< raw baro data subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ @@ -162,6 +165,7 @@ private: float rev[_rc_max_chan_count]; float dz[_rc_max_chan_count]; float ex[_rc_max_chan_count]; + float scaling_factor[_rc_max_chan_count]; float gyro_offset[3]; float mag_offset[3]; @@ -331,6 +335,7 @@ Sensors::Sensors() : _gyro_sub(-1), _accel_sub(-1), _mag_sub(-1), + _rc_sub(-1), _baro_sub(-1), _vstatus_sub(-1), _params_sub(-1), @@ -464,14 +469,13 @@ Sensors::parameters_update() warnx("Failed getting exponential gain for chan %d", i); } - _rc.chan[i].scaling_factor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); + _parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); /* handle blowup in the scaling factor calculation */ - if (isnan(_rc.chan[i].scaling_factor) || isinf(_rc.chan[i].scaling_factor)) { - _rc.chan[i].scaling_factor = 0; + if (isnan(_parameters.scaling_factor[i]) || isinf(_parameters.scaling_factor[i])) { + _parameters.scaling_factor[i] = 0; } - _rc.chan[i].mid = _parameters.trim[i]; } /* update RC function mappings */ @@ -856,98 +860,125 @@ Sensors::adc_poll(struct sensor_combined_s &raw) void Sensors::ppm_poll() { - struct manual_control_setpoint_s manual_control; - - /* check to see whether a new frame has been decoded */ - if (_ppm_last_valid == ppm_last_valid_decode) - return; - /* require at least two chanels to consider the signal valid */ - if (ppm_decoded_channels < 4) - return; - - unsigned channel_limit = ppm_decoded_channels; - if (channel_limit > _rc_max_chan_count) - channel_limit = _rc_max_chan_count; - - /* we are accepting this decode */ - _ppm_last_valid = ppm_last_valid_decode; - - /* Read out values from HRT */ - for (unsigned int i = 0; i < channel_limit; i++) { - _rc.chan[i].raw = ppm_buffer[i]; - - /* scale around the mid point differently for lower and upper range */ - if (ppm_buffer[i] > (_parameters.trim[i] + _parameters.dz[i])) { - _rc.chan[i].scaled = (ppm_buffer[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]); - } else if (ppm_buffer[i] < (_parameters.trim[i] - _parameters.dz[i])) { - /* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */ - _rc.chan[i].scaled = -((_parameters.trim[i] - ppm_buffer[i]) / (float)(_parameters.trim[i] - _parameters.min[i])); - - } else { - /* in the configured dead zone, output zero */ - _rc.chan[i].scaled = 0.0f; + /* fake low-level driver, directly pulling from driver variables */ + static orb_advert_t rc_input_pub = -1; + struct rc_input_values raw; + + raw.timestamp = ppm_last_valid_decode; + + if (ppm_decoded_channels > 1) { + + for (int i = 0; i < ppm_decoded_channels; i++) { + raw.values[i] = ppm_buffer[i]; } - /* reverse channel if required */ - if (i == _rc.function[THROTTLE]) { - if ((int)_parameters.rev[i] == -1) { - _rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled; - } + raw.channel_count = ppm_decoded_channels; + + /* publish to object request broker */ + if (rc_input_pub <= 0) { + rc_input_pub = orb_advertise(ORB_ID(input_rc), &raw); } else { - _rc.chan[i].scaled *= _parameters.rev[i]; + orb_publish(ORB_ID(input_rc), rc_input_pub, &raw); } + } - /* handle any parameter-induced blowups */ - if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled)) - _rc.chan[i].scaled = 0.0f; - //_rc.chan[i].scaled = (ppm_buffer[i] - _rc.chan[i].mid) * _rc.chan[i].scaling_factor; - } + /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ + bool rc_updated; + orb_check(_rc_sub, &rc_updated); - _rc.chan_count = ppm_decoded_channels; - _rc.timestamp = ppm_last_valid_decode; + if (rc_updated) { + struct rc_input_values rc_input; - manual_control.timestamp = ppm_last_valid_decode; + orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); - /* roll input - rolling right is stick-wise and rotation-wise positive */ - manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled; - if (manual_control.roll < -1.0f) manual_control.roll = -1.0f; - if (manual_control.roll > 1.0f) manual_control.roll = 1.0f; - if (!isnan(_parameters.rc_scale_roll) || !isinf(_parameters.rc_scale_roll)) { - manual_control.roll *= _parameters.rc_scale_roll; - } + struct manual_control_setpoint_s manual_control; - /* - * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, - * so reverse sign. - */ - manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled; - if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f; - if (manual_control.pitch > 1.0f) manual_control.pitch = 1.0f; - if (!isnan(_parameters.rc_scale_pitch) || !isinf(_parameters.rc_scale_pitch)) { - manual_control.pitch *= _parameters.rc_scale_pitch; - } + /* require at least two chanels to consider the signal valid */ + if (rc_input.channel_count < 2) + return; - /* yaw input - stick right is positive and positive rotation */ - manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled * _parameters.rc_scale_yaw; - if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f; - if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f; - if (!isnan(_parameters.rc_scale_yaw) || !isinf(_parameters.rc_scale_yaw)) { - manual_control.yaw *= _parameters.rc_scale_yaw; - } - - /* throttle input */ - manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; - if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; - if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; + unsigned channel_limit = rc_input.channel_count; + if (channel_limit > _rc_max_chan_count) + channel_limit = _rc_max_chan_count; - /* mode switch input */ - manual_control.override_mode_switch = _rc.chan[_rc.function[OVERRIDE]].scaled; - if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f; - if (manual_control.override_mode_switch > 1.0f) manual_control.override_mode_switch = 1.0f; + /* we are accepting this message */ + _ppm_last_valid = rc_input.timestamp; - orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); - orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); + /* Read out values from raw message */ + for (unsigned int i = 0; i < channel_limit; i++) { + + /* scale around the mid point differently for lower and upper range */ + if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) { + _rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]); + } else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) { + /* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */ + _rc.chan[i].scaled = -((_parameters.trim[i] - rc_input.values[i]) / (float)(_parameters.trim[i] - _parameters.min[i])); + + } else { + /* in the configured dead zone, output zero */ + _rc.chan[i].scaled = 0.0f; + } + + /* reverse channel if required */ + if (i == _rc.function[THROTTLE]) { + if ((int)_parameters.rev[i] == -1) { + _rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled; + } + } else { + _rc.chan[i].scaled *= _parameters.rev[i]; + } + + /* handle any parameter-induced blowups */ + if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled)) + _rc.chan[i].scaled = 0.0f; + } + + _rc.chan_count = rc_input.channel_count; + _rc.timestamp = rc_input.timestamp; + + manual_control.timestamp = rc_input.timestamp; + + /* roll input - rolling right is stick-wise and rotation-wise positive */ + manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled; + if (manual_control.roll < -1.0f) manual_control.roll = -1.0f; + if (manual_control.roll > 1.0f) manual_control.roll = 1.0f; + if (!isnan(_parameters.rc_scale_roll) || !isinf(_parameters.rc_scale_roll)) { + manual_control.roll *= _parameters.rc_scale_roll; + } + + /* + * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, + * so reverse sign. + */ + manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled; + if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f; + if (manual_control.pitch > 1.0f) manual_control.pitch = 1.0f; + if (!isnan(_parameters.rc_scale_pitch) || !isinf(_parameters.rc_scale_pitch)) { + manual_control.pitch *= _parameters.rc_scale_pitch; + } + + /* yaw input - stick right is positive and positive rotation */ + manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled * _parameters.rc_scale_yaw; + if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f; + if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f; + if (!isnan(_parameters.rc_scale_yaw) || !isinf(_parameters.rc_scale_yaw)) { + manual_control.yaw *= _parameters.rc_scale_yaw; + } + + /* throttle input */ + manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; + if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; + if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; + + /* mode switch input */ + manual_control.override_mode_switch = _rc.chan[_rc.function[OVERRIDE]].scaled; + if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f; + if (manual_control.override_mode_switch > 1.0f) manual_control.override_mode_switch = 1.0f; + + orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); + orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); + } } #endif @@ -979,6 +1010,7 @@ Sensors::task_main() _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); + _rc_sub = orb_subscribe(ORB_ID(input_rc)); _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); diff --git a/apps/systemlib/geo/geo.c b/apps/systemlib/geo/geo.c index 3709feb15..abe44c69e 100644 --- a/apps/systemlib/geo/geo.c +++ b/apps/systemlib/geo/geo.c @@ -47,6 +47,150 @@ #include <systemlib/geo/geo.h> #include <math.h> + +/* values for map projection */ +static double phi_1; +static double sin_phi_1; +static double cos_phi_1; +static double lambda_0; +static double scale; + +/** + * Initializes the map transformation. + * + * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +__EXPORT static void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +{ + /* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ + phi_1 = lat_0 / 180.0 * M_PI; + lambda_0 = lon_0 / 180.0 * M_PI; + + sin_phi_1 = sin(phi_1); + cos_phi_1 = cos(phi_1); + + /* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale + + /* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */ + const double r_earth = 6371000; + + double lat1 = phi_1; + double lon1 = lambda_0; + + double lat2 = phi_1 + 0.5 / 180 * M_PI; + double lon2 = lambda_0 + 0.5 / 180 * M_PI; + double sin_lat_2 = sin(lat2); + double cos_lat_2 = cos(lat2); + double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth; + + /* 2) calculate distance rho on plane */ + double k_bar = 0; + double c = acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)); + + if (0 != c) + k_bar = c / sin(c); + + double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane + double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0))); + double rho = sqrt(pow(x2, 2) + pow(y2, 2)); + + scale = d / rho; + +} + +/** + * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +__EXPORT static void map_projection_project(double lat, double lon, float *x, float *y) +{ + /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ + double phi = lat / 180.0 * M_PI; + double lambda = lon / 180.0 * M_PI; + + double sin_phi = sin(phi); + double cos_phi = cos(phi); + + double k_bar = 0; + /* using small angle approximation (formula in comment is without aproximation) */ + double c = acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c = acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) ); + + if (0 != c) + k_bar = c / sin(c); + + /* using small angle approximation (formula in comment is without aproximation) */ + *y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale; + *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; // *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale; + +// printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0); +} + +/** + * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system + * + * @param x north + * @param y east + * @param lat in degrees (47.1234567°, not 471234567°) + * @param lon in degrees (8.1234567°, not 81234567°) + */ +__EXPORT static void map_projection_reproject(float x, float y, double *lat, double *lon) +{ + /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ + + double x_descaled = x / scale; + double y_descaled = y / scale; + + double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2)); + double sin_c = sin(c); + double cos_c = cos(c); + + double lat_sphere = 0; + + if (c != 0) + lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c); + else + lat_sphere = asin(cos_c * sin_phi_1); + +// printf("lat_sphere = %.10f\n",lat_sphere); + + double lon_sphere = 0; + + if (phi_1 == M_PI / 2) { + //using small angle approximation (formula in comment is without aproximation) + lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled)); + + } else if (phi_1 == -M_PI / 2) { + //using small angle approximation (formula in comment is without aproximation) + lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled)); + + } else { + + lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c)); + //using small angle approximation +// double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c); +// if(denominator != 0) +// { +// lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator); +// } +// else +// { +// ... +// } + } + +// printf("lon_sphere = %.10f\n",lon_sphere); + + *lat = lat_sphere * 180.0 / M_PI; + *lon = lon_sphere * 180.0 / M_PI; + +} + + __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) { double lat_now_rad = lat_now / 180.0d * M_PI; diff --git a/apps/systemlib/geo/geo.h b/apps/systemlib/geo/geo.h index c98b4c85d..807e508a2 100644 --- a/apps/systemlib/geo/geo.h +++ b/apps/systemlib/geo/geo.h @@ -55,6 +55,12 @@ typedef struct { float bearing; // Bearing in radians to closest point on line/arc } crosstrack_error_s; +__EXPORT static void map_projection_init(double lat_0, double lon_0); + +__EXPORT static void map_projection_project(double lat, double lon, float *x, float *y); + +__EXPORT static void map_projection_reproject(float x, float y, double *lat, double *lon); + __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); @@ -69,4 +75,4 @@ __EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, float _wrap180(float bearing); float _wrap360(float bearing); float _wrapPI(float bearing); -float _wrap2PI(float bearing);
\ No newline at end of file +float _wrap2PI(float bearing); diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c index dd8b6adc6..0358caa25 100644 --- a/apps/systemlib/pid/pid.c +++ b/apps/systemlib/pid/pid.c @@ -172,9 +172,9 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculate the output. Limit output magnitude to pid->limit float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd); - //if (output > pid->limit) output = pid->limit; + if (output > pid->limit) output = pid->limit; - //if (output < -pid->limit) output = -pid->limit; + if (output < -pid->limit) output = -pid->limit; if (isfinite(output)) { pid->last_output = output; diff --git a/apps/uORB/topics/rc_channels.h b/apps/uORB/topics/rc_channels.h index 2c487cf3b..fef6ef2b3 100644 --- a/apps/uORB/topics/rc_channels.h +++ b/apps/uORB/topics/rc_channels.h @@ -50,14 +50,6 @@ * @{ */ -enum RC_CHANNELS_STATUS -{ - UNKNOWN = 0, - KNOWN = 1, - SIGNAL = 2, - TIMEOUT = 3 -}; - /** * This defines the mapping of the RC functions. * The value assigned to the specific function corresponds to the entry of @@ -85,12 +77,7 @@ struct rc_channels_s { uint64_t timestamp; /**< In microseconds since boot time. */ uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */ struct { - uint16_t mid; /**< midpoint (0). */ - float scaling_factor; /**< scaling factor from raw counts to -1..+1 */ - uint16_t raw; /**< current raw value */ float scaled; /**< Scaled to -1..1 (throttle: 0..1) */ - uint16_t override; - enum RC_CHANNELS_STATUS status; /**< status of the channel */ } chan[RC_CHANNELS_FUNCTION_MAX]; uint8_t chan_count; /**< maximum number of valid channels */ @@ -98,6 +85,7 @@ struct rc_channels_s { char function_name[RC_CHANNELS_FUNCTION_MAX][20]; uint8_t function[RC_CHANNELS_FUNCTION_MAX]; uint8_t rssi; /**< Overall receive signal strength */ + bool is_valid; /**< Inputs are valid, no timeout */ }; /**< radio control channels. */ /** diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig index 64b521e21..e90e7ce62 100755 --- a/nuttx/configs/px4io/io/defconfig +++ b/nuttx/configs/px4io/io/defconfig @@ -155,6 +155,8 @@ CONFIG_STM32_ADC3=n # CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity # CONFIG_USARTn_2STOP - Two stop bits # +CONFIG_SERIAL_TERMIOS=y + CONFIG_USART1_SERIAL_CONSOLE=y CONFIG_USART2_SERIAL_CONSOLE=n CONFIG_USART3_SERIAL_CONSOLE=n @@ -163,11 +165,11 @@ CONFIG_USART1_TXBUFSIZE=32 CONFIG_USART2_TXBUFSIZE=32 CONFIG_USART3_TXBUFSIZE=32 -CONFIG_USART1_RXBUFSIZE=32 +CONFIG_USART1_RXBUFSIZE=64 CONFIG_USART2_RXBUFSIZE=128 CONFIG_USART3_RXBUFSIZE=32 -CONFIG_USART1_BAUD=57600 +CONFIG_USART1_BAUD=115200 CONFIG_USART2_BAUD=115200 CONFIG_USART3_BAUD=115200 |