aboutsummaryrefslogtreecommitdiff
path: root/src/modules/mavlink
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r--src/modules/mavlink/mavlink.c167
-rw-r--r--src/modules/mavlink/mavlink_log.c129
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp93
-rw-r--r--src/modules/mavlink/module.mk1
-rw-r--r--src/modules/mavlink/orb_listener.c89
-rw-r--r--src/modules/mavlink/orb_topics.h6
-rw-r--r--src/modules/mavlink/util.h2
-rw-r--r--src/modules/mavlink/waypoints.c40
8 files changed, 253 insertions, 274 deletions
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 919d01561..d7b0fa9c7 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -64,6 +64,7 @@
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <mavlink/mavlink_log.h>
+#include <commander/px4_custom_mode.h>
#include "waypoints.h"
#include "orb_topics.h"
@@ -181,102 +182,80 @@ set_hil_on_off(bool hil_enabled)
}
void
-get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
+get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
{
/* reset MAVLink mode bitfield */
- *mavlink_mode = 0;
+ *mavlink_base_mode = 0;
+ *mavlink_custom_mode = 0;
- /* set mode flags independent of system state */
+ /**
+ * Set mode flags
+ **/
/* HIL */
- if (v_status.flag_hil_enabled) {
- *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
+ if (v_status.hil_state == HIL_STATE_ON) {
+ *mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
}
- /* manual input */
- if (v_status.flag_control_manual_enabled) {
- *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+ /* arming state */
+ if (v_status.arming_state == ARMING_STATE_ARMED
+ || v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
+ *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
}
- /* attitude or rate control */
- if (v_status.flag_control_attitude_enabled ||
- v_status.flag_control_rates_enabled) {
- *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
- }
-
- /* vector control */
- if (v_status.flag_control_velocity_enabled ||
- v_status.flag_control_position_enabled) {
- *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
- }
-
- /* autonomous mode */
- if (v_status.state_machine == SYSTEM_STATE_AUTO) {
- *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
- }
-
- /* set arming state */
- if (armed.armed) {
- *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
-
- } else {
- *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
- }
-
- switch (v_status.state_machine) {
- case SYSTEM_STATE_PREFLIGHT:
- if (v_status.flag_preflight_gyro_calibration ||
- v_status.flag_preflight_mag_calibration ||
- v_status.flag_preflight_accel_calibration) {
- *mavlink_state = MAV_STATE_CALIBRATING;
-
- } else {
- *mavlink_state = MAV_STATE_UNINIT;
+ /* main state */
+ *mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+ union px4_custom_mode custom_mode;
+ custom_mode.data = 0;
+ if (v_status.main_state == MAIN_STATE_MANUAL) {
+ *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
+ custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+ } else if (v_status.main_state == MAIN_STATE_SEATBELT) {
+ *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
+ custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
+ } else if (v_status.main_state == MAIN_STATE_EASY) {
+ *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+ custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
+ } else if (v_status.main_state == MAIN_STATE_AUTO) {
+ *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+ custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+ if (v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) {
+ custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
+ } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
+ custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
+ } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER) {
+ custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
+ } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) {
+ custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
+ } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL) {
+ custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
+ } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) {
+ custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
}
-
- break;
-
- case SYSTEM_STATE_STANDBY:
- *mavlink_state = MAV_STATE_STANDBY;
- break;
-
- case SYSTEM_STATE_GROUND_READY:
- *mavlink_state = MAV_STATE_ACTIVE;
- break;
-
- case SYSTEM_STATE_MANUAL:
- *mavlink_state = MAV_STATE_ACTIVE;
- break;
-
- case SYSTEM_STATE_STABILIZED:
- *mavlink_state = MAV_STATE_ACTIVE;
- break;
-
- case SYSTEM_STATE_AUTO:
+ }
+ *mavlink_custom_mode = custom_mode.data;
+
+ /**
+ * Set mavlink state
+ **/
+
+ /* set calibration state */
+ if (v_status.arming_state == ARMING_STATE_INIT
+ || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE
+ || v_status.arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review
+ *mavlink_state = MAV_STATE_UNINIT;
+ } else if (v_status.arming_state == ARMING_STATE_ARMED) {
*mavlink_state = MAV_STATE_ACTIVE;
- break;
-
- case SYSTEM_STATE_MISSION_ABORT:
- *mavlink_state = MAV_STATE_EMERGENCY;
- break;
-
- case SYSTEM_STATE_EMCY_LANDING:
- *mavlink_state = MAV_STATE_EMERGENCY;
- break;
-
- case SYSTEM_STATE_EMCY_CUTOFF:
- *mavlink_state = MAV_STATE_EMERGENCY;
- break;
-
- case SYSTEM_STATE_GROUND_ERROR:
- *mavlink_state = MAV_STATE_EMERGENCY;
- break;
-
- case SYSTEM_STATE_REBOOT:
+ } else if (v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
+ *mavlink_state = MAV_STATE_CRITICAL;
+ } else if (v_status.arming_state == ARMING_STATE_STANDBY) {
+ *mavlink_state = MAV_STATE_STANDBY;
+ } else if (v_status.arming_state == ARMING_STATE_REBOOT) {
*mavlink_state = MAV_STATE_POWEROFF;
- break;
+ } else {
+ warnx("Unknown mavlink state");
+ *mavlink_state = MAV_STATE_CRITICAL;
}
-
}
@@ -312,6 +291,9 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m
orb_set_interval(subs->act_2_sub, min_interval);
orb_set_interval(subs->act_3_sub, min_interval);
orb_set_interval(subs->actuators_sub, min_interval);
+ orb_set_interval(subs->actuators_effective_sub, min_interval);
+ orb_set_interval(subs->spa_sub, min_interval);
+ orb_set_interval(mavlink_subs.rates_setpoint_sub, min_interval);
break;
case MAVLINK_MSG_ID_MANUAL_CONTROL:
@@ -568,6 +550,7 @@ int mavlink_thread_main(int argc, char *argv[])
default:
usage();
+ break;
}
}
@@ -674,23 +657,27 @@ int mavlink_thread_main(int argc, char *argv[])
/* translate the current system state to mavlink state and mode */
uint8_t mavlink_state = 0;
- uint8_t mavlink_mode = 0;
- get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
+ uint8_t mavlink_base_mode = 0;
+ uint32_t mavlink_custom_mode = 0;
+ get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
/* send heartbeat */
- mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
+ mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state);
/* switch HIL mode if required */
- set_hil_on_off(v_status.flag_hil_enabled);
+ if (v_status.hil_state == HIL_STATE_ON)
+ set_hil_on_off(true);
+ else if (v_status.hil_state == HIL_STATE_OFF)
+ set_hil_on_off(false);
/* send status (values already copied in the section above) */
mavlink_msg_sys_status_send(chan,
v_status.onboard_control_sensors_present,
v_status.onboard_control_sensors_enabled,
v_status.onboard_control_sensors_health,
- v_status.load,
- v_status.voltage_battery * 1000.0f,
- v_status.current_battery * 1000.0f,
+ v_status.load * 1000.0f,
+ v_status.battery_voltage * 1000.0f,
+ v_status.battery_current * 1000.0f,
v_status.battery_remaining,
v_status.drop_rate_comm,
v_status.errors_comm,
diff --git a/src/modules/mavlink/mavlink_log.c b/src/modules/mavlink/mavlink_log.c
deleted file mode 100644
index 192195856..000000000
--- a/src/modules/mavlink/mavlink_log.c
+++ /dev/null
@@ -1,129 +0,0 @@
-/****************************************************************************
- *
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
- * Author: Lorenz Meier <lm@inf.ethz.ch>
- *
- * 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 mavlink_log.c
- * MAVLink text logging.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-#include <string.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <stdarg.h>
-
-#include <mavlink/mavlink_log.h>
-
-static FILE* text_recorder_fd = NULL;
-
-void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size)
-{
- lb->size = size;
- lb->start = 0;
- lb->count = 0;
- lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage));
- text_recorder_fd = fopen("/fs/microsd/text_recorder.txt", "w");
-}
-
-int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb)
-{
- return lb->count == (int)lb->size;
-}
-
-int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb)
-{
- return lb->count == 0;
-}
-
-void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem)
-{
- int end = (lb->start + lb->count) % lb->size;
- memcpy(&(lb->elems[end]), elem, sizeof(struct mavlink_logmessage));
-
- if (mavlink_logbuffer_is_full(lb)) {
- lb->start = (lb->start + 1) % lb->size; /* full, overwrite */
-
- } else {
- ++lb->count;
- }
-}
-
-int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem)
-{
- if (!mavlink_logbuffer_is_empty(lb)) {
- memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage));
- lb->start = (lb->start + 1) % lb->size;
- --lb->count;
-
- if (text_recorder_fd) {
- fwrite(elem->text, 1, strnlen(elem->text, 50), text_recorder_fd);
- fputc("\n", text_recorder_fd);
- fsync(text_recorder_fd);
- }
-
- return 0;
-
- } else {
- return 1;
- }
-}
-
-void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...)
-{
- va_list ap;
- va_start(ap, fmt);
- int end = (lb->start + lb->count) % lb->size;
- lb->elems[end].severity = severity;
- vsnprintf(lb->elems[end].text, sizeof(lb->elems[0].text), fmt, ap);
- va_end(ap);
-
- /* increase count */
- if (mavlink_logbuffer_is_full(lb)) {
- lb->start = (lb->start + 1) % lb->size; /* full, overwrite */
-
- } else {
- ++lb->count;
- }
-}
-
-__EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...)
-{
- va_list ap;
- va_start(ap, fmt);
- char text[MAVLINK_LOG_MAXLEN + 1];
- vsnprintf(text, sizeof(text), fmt, ap);
- va_end(ap);
- ioctl(_fd, severity, (unsigned long)&text[0]);
-}
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 01bbabd46..af43542da 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -50,6 +50,10 @@
#include <mqueue.h>
#include <string.h>
#include <drivers/drv_hrt.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_mag.h>
+#include <drivers/drv_baro.h>
#include <time.h>
#include <float.h>
#include <unistd.h>
@@ -67,6 +71,7 @@
#include <systemlib/err.h>
#include <systemlib/airspeed.h>
#include <mavlink/mavlink_log.h>
+#include <commander/px4_custom_mode.h>
__BEGIN_DECLS
@@ -101,6 +106,10 @@ static orb_advert_t pub_hil_global_pos = -1;
static orb_advert_t pub_hil_attitude = -1;
static orb_advert_t pub_hil_gps = -1;
static orb_advert_t pub_hil_sensors = -1;
+static orb_advert_t pub_hil_gyro = -1;
+static orb_advert_t pub_hil_accel = -1;
+static orb_advert_t pub_hil_mag = -1;
+static orb_advert_t pub_hil_baro = -1;
static orb_advert_t pub_hil_airspeed = -1;
static orb_advert_t cmd_pub = -1;
@@ -188,9 +197,11 @@ handle_message(mavlink_message_t *msg)
mavlink_set_mode_t new_mode;
mavlink_msg_set_mode_decode(msg, &new_mode);
+ union px4_custom_mode custom_mode;
+ custom_mode.data = new_mode.custom_mode;
/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
vcmd.param1 = new_mode.base_mode;
- vcmd.param2 = new_mode.custom_mode;
+ vcmd.param2 = custom_mode.main_mode;
vcmd.param3 = 0;
vcmd.param4 = 0;
vcmd.param5 = 0;
@@ -412,12 +423,12 @@ handle_message(mavlink_message_t *msg)
/* airspeed from differential pressure, ambient pressure and temp */
struct airspeed_s airspeed;
- airspeed.timestamp = hrt_absolute_time();
float ias = calc_indicated_airspeed(imu.diff_pressure);
// XXX need to fix this
float tas = ias;
+ airspeed.timestamp = hrt_absolute_time();
airspeed.indicated_airspeed_m_s = ias;
airspeed.true_airspeed_m_s = tas;
@@ -428,7 +439,67 @@ handle_message(mavlink_message_t *msg)
}
//warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s);
- /* publish */
+ /* individual sensor publications */
+ struct gyro_report gyro;
+ gyro.x_raw = imu.xgyro / mrad2rad;
+ gyro.y_raw = imu.ygyro / mrad2rad;
+ gyro.z_raw = imu.zgyro / mrad2rad;
+ gyro.x = imu.xgyro;
+ gyro.y = imu.ygyro;
+ gyro.z = imu.zgyro;
+ gyro.temperature = imu.temperature;
+ gyro.timestamp = hrt_absolute_time();
+
+ if (pub_hil_gyro < 0) {
+ pub_hil_gyro = orb_advertise(ORB_ID(sensor_gyro), &gyro);
+ } else {
+ orb_publish(ORB_ID(sensor_gyro), pub_hil_gyro, &gyro);
+ }
+
+ struct accel_report accel;
+ accel.x_raw = imu.xacc / mg2ms2;
+ accel.y_raw = imu.yacc / mg2ms2;
+ accel.z_raw = imu.zacc / mg2ms2;
+ accel.x = imu.xacc;
+ accel.y = imu.yacc;
+ accel.z = imu.zacc;
+ accel.temperature = imu.temperature;
+ accel.timestamp = hrt_absolute_time();
+
+ if (pub_hil_accel < 0) {
+ pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel);
+ } else {
+ orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
+ }
+
+ struct mag_report mag;
+ mag.x_raw = imu.xmag / mga2ga;
+ mag.y_raw = imu.ymag / mga2ga;
+ mag.z_raw = imu.zmag / mga2ga;
+ mag.x = imu.xmag;
+ mag.y = imu.ymag;
+ mag.z = imu.zmag;
+ mag.timestamp = hrt_absolute_time();
+
+ if (pub_hil_mag < 0) {
+ pub_hil_mag = orb_advertise(ORB_ID(sensor_mag), &mag);
+ } else {
+ orb_publish(ORB_ID(sensor_mag), pub_hil_mag, &mag);
+ }
+
+ struct baro_report baro;
+ baro.pressure = imu.abs_pressure;
+ baro.altitude = imu.pressure_alt;
+ baro.temperature = imu.temperature;
+ baro.timestamp = hrt_absolute_time();
+
+ if (pub_hil_baro < 0) {
+ pub_hil_baro = orb_advertise(ORB_ID(sensor_baro), &baro);
+ } else {
+ orb_publish(ORB_ID(sensor_baro), pub_hil_baro, &baro);
+ }
+
+ /* publish combined sensor topic */
if (pub_hil_sensors > 0) {
orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
} else {
@@ -552,6 +623,22 @@ handle_message(mavlink_message_t *msg)
} else {
pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
}
+
+ struct accel_report accel;
+ accel.x_raw = hil_state.xacc / 9.81f * 1e3f;
+ accel.y_raw = hil_state.yacc / 9.81f * 1e3f;
+ accel.z_raw = hil_state.zacc / 9.81f * 1e3f;
+ accel.x = hil_state.xacc;
+ accel.y = hil_state.yacc;
+ accel.z = hil_state.zacc;
+ accel.temperature = 25.0f;
+ accel.timestamp = hrt_absolute_time();
+
+ if (pub_hil_accel < 0) {
+ pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel);
+ } else {
+ orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
+ }
}
if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index bfccb2d38..5d3d6a73c 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -39,7 +39,6 @@ MODULE_COMMAND = mavlink
SRCS += mavlink.c \
missionlib.c \
mavlink_parameters.c \
- mavlink_log.c \
mavlink_receiver.cpp \
orb_listener.c \
waypoints.c
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index edb8761b8..53d86ec00 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -71,7 +71,8 @@ struct vehicle_status_s v_status;
struct rc_channels_s rc;
struct rc_input_values rc_raw;
struct actuator_armed_s armed;
-struct actuator_controls_effective_s actuators_0;
+struct actuator_controls_effective_s actuators_effective_0;
+struct actuator_controls_s actuators_0;
struct vehicle_attitude_s att;
struct mavlink_subscriptions mavlink_subs;
@@ -112,6 +113,7 @@ static void l_attitude_setpoint(const struct listener *l);
static void l_actuator_outputs(const struct listener *l);
static void l_actuator_armed(const struct listener *l);
static void l_manual_control_setpoint(const struct listener *l);
+static void l_vehicle_attitude_controls_effective(const struct listener *l);
static void l_vehicle_attitude_controls(const struct listener *l);
static void l_debug_key_value(const struct listener *l);
static void l_optical_flow(const struct listener *l);
@@ -138,6 +140,7 @@ static const struct listener listeners[] = {
{l_actuator_armed, &mavlink_subs.armed_sub, 0},
{l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0},
{l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0},
+ {l_vehicle_attitude_controls_effective, &mavlink_subs.actuators_effective_sub, 0},
{l_debug_key_value, &mavlink_subs.debug_key_value, 0},
{l_optical_flow, &mavlink_subs.optical_flow, 0},
{l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0},
@@ -272,19 +275,23 @@ l_vehicle_status(const struct listener *l)
orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
/* enable or disable HIL */
- set_hil_on_off(v_status.flag_hil_enabled);
+ if (v_status.hil_state == HIL_STATE_ON)
+ set_hil_on_off(true);
+ else if (v_status.hil_state == HIL_STATE_OFF)
+ set_hil_on_off(false);
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
- uint8_t mavlink_mode = 0;
- get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
+ uint8_t mavlink_base_mode = 0;
+ uint32_t mavlink_custom_mode = 0;
+ get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
/* send heartbeat */
mavlink_msg_heartbeat_send(chan,
mavlink_system.type,
MAV_AUTOPILOT_PX4,
- mavlink_mode,
- v_status.state_machine,
+ mavlink_base_mode,
+ mavlink_custom_mode,
mavlink_state);
}
@@ -334,7 +341,7 @@ l_global_position(const struct listener *l)
int16_t vz = (int16_t)(global_pos.vz * 100.0f);
/* heading in degrees * 10, from 0 to 36.000) */
- uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f);
+ uint16_t hdg = (global_pos.yaw / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f);
mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
timestamp / 1000,
@@ -470,8 +477,9 @@ l_actuator_outputs(const struct listener *l)
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
- uint8_t mavlink_mode = 0;
- get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
+ uint8_t mavlink_base_mode = 0;
+ uint32_t mavlink_custom_mode = 0;
+ get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
/* HIL message as per MAVLink spec */
@@ -488,7 +496,7 @@ l_actuator_outputs(const struct listener *l)
-1,
-1,
-1,
- mavlink_mode,
+ mavlink_base_mode,
0);
} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
@@ -502,7 +510,7 @@ l_actuator_outputs(const struct listener *l)
((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
-1,
-1,
- mavlink_mode,
+ mavlink_base_mode,
0);
} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
@@ -516,7 +524,7 @@ l_actuator_outputs(const struct listener *l)
((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f,
- mavlink_mode,
+ mavlink_base_mode,
0);
} else {
@@ -530,7 +538,7 @@ l_actuator_outputs(const struct listener *l)
(act_outputs.output[5] - 1500.0f) / 500.0f,
(act_outputs.output[6] - 1500.0f) / 500.0f,
(act_outputs.output[7] - 1500.0f) / 500.0f,
- mavlink_mode,
+ mavlink_base_mode,
0);
}
}
@@ -562,28 +570,54 @@ l_manual_control_setpoint(const struct listener *l)
}
void
-l_vehicle_attitude_controls(const struct listener *l)
+l_vehicle_attitude_controls_effective(const struct listener *l)
{
- orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators_0);
+ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_effective_sub, &actuators_effective_0);
if (gcs_link) {
/* send, add spaces so that string buffer is at least 10 chars long */
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl0 ",
- actuators_0.control_effective[0]);
+ actuators_effective_0.control_effective[0]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl1 ",
- actuators_0.control_effective[1]);
+ actuators_effective_0.control_effective[1]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl2 ",
- actuators_0.control_effective[2]);
+ actuators_effective_0.control_effective[2]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
"eff ctrl3 ",
- actuators_0.control_effective[3]);
+ actuators_effective_0.control_effective[3]);
+ }
+}
+
+void
+l_vehicle_attitude_controls(const struct listener *l)
+{
+ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0);
+
+ if (gcs_link) {
+ /* send, add spaces so that string buffer is at least 10 chars long */
+ mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+ last_sensor_timestamp / 1000,
+ "ctrl0 ",
+ actuators_0.control[0]);
+ mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+ last_sensor_timestamp / 1000,
+ "ctrl1 ",
+ actuators_0.control[1]);
+ mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+ last_sensor_timestamp / 1000,
+ "ctrl2 ",
+ actuators_0.control[2]);
+ mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+ last_sensor_timestamp / 1000,
+ "ctrl3 ",
+ actuators_0.control[3]);
}
}
@@ -632,12 +666,12 @@ l_airspeed(const struct listener *l)
orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed);
float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
- float throttle = actuators_0.control_effective[3] * (UINT16_MAX - 1);
- float alt = global_pos.alt;
- float climb = global_pos.vz;
+ uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f;
+ float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1);
+ float alt = global_pos.relative_alt;
+ float climb = -global_pos.vz;
- mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed,
- ((att.yaw + M_PI_F) / M_PI_F) * 180.0f, throttle, alt, climb);
+ mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb);
}
static void *
@@ -673,7 +707,7 @@ uorb_receive_thread(void *arg)
/* handle the poll result */
if (poll_ret == 0) {
- mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s");
+ /* silent */
} else if (poll_ret < 0) {
mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data");
@@ -768,7 +802,10 @@ uorb_receive_start(void)
orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */
/* --- ACTUATOR CONTROL VALUE --- */
- mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
+ mavlink_subs.actuators_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
+ orb_set_interval(mavlink_subs.actuators_effective_sub, 100); /* 10Hz updates */
+
+ mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */
/* --- DEBUG VALUE OUTPUT --- */
diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h
index 73e278dc6..e2e630046 100644
--- a/src/modules/mavlink/orb_topics.h
+++ b/src/modules/mavlink/orb_topics.h
@@ -55,10 +55,12 @@
#include <uORB/topics/vehicle_global_position_set_triplet.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/debug_key_value.h>
@@ -75,8 +77,10 @@ struct mavlink_subscriptions {
int act_3_sub;
int gps_sub;
int man_control_sp_sub;
- int armed_sub;
+ int safety_sub;
int actuators_sub;
+ int armed_sub;
+ int actuators_effective_sub;
int local_pos_sub;
int spa_sub;
int spl_sub;
diff --git a/src/modules/mavlink/util.h b/src/modules/mavlink/util.h
index a4ff06a88..5e5ee8261 100644
--- a/src/modules/mavlink/util.h
+++ b/src/modules/mavlink/util.h
@@ -51,4 +51,4 @@ extern mavlink_wpm_storage *wpm;
/**
* Translate the custom state into standard mavlink modes and state.
*/
-extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode);
+extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index eea928a17..16a7c2d35 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -294,16 +294,13 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq)
*/
float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt)
{
- //TODO: implement for z once altidude contoller is implemented
-
static uint16_t counter;
-// if(counter % 10 == 0) printf(" x = %.10f, y = %.10f\n", x, y);
if (seq < wpm->size) {
- mavlink_mission_item_t *cur = &(wpm->waypoints[seq]);
+ mavlink_mission_item_t *wp = &(wpm->waypoints[seq]);
- double current_x_rad = cur->x / 180.0 * M_PI;
- double current_y_rad = cur->y / 180.0 * M_PI;
+ double current_x_rad = wp->x / 180.0 * M_PI;
+ double current_y_rad = wp->y / 180.0 * M_PI;
double x_rad = lat / 180.0 * M_PI;
double y_rad = lon / 180.0 * M_PI;
@@ -315,7 +312,10 @@ float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float
const double radius_earth = 6371000.0;
- return radius_earth * c;
+ float dxy = radius_earth * c;
+ float dz = alt - wp->z;
+
+ return sqrtf(dxy * dxy + dz * dz);
} else {
return -1.0f;
@@ -383,21 +383,19 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
// XXX TODO
}
- if (dist >= 0.f && dist <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw
-
+ if (dist >= 0.f && dist <= orbit) {
wpm->pos_reached = true;
-
}
-
-// else
-// {
-// if(counter % 100 == 0)
-// printf("Setpoint not reached yet: %0.4f, orbit: %.4f, coordinate frame: %d\n",dist, orbit, coordinate_frame);
-// }
+ // check if required yaw reached
+ float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI);
+ float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw);
+ if (fabsf(yaw_err) < 0.05f) {
+ wpm->yaw_reached = true;
+ }
}
//check if the current waypoint was reached
- if (wpm->pos_reached /*wpm->yaw_reached &&*/ && !wpm->idle) {
+ if (wpm->pos_reached && /*wpm->yaw_reached &&*/ !wpm->idle) {
if (wpm->current_active_wp_id < wpm->size) {
mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]);
@@ -412,11 +410,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
bool time_elapsed = false;
- if (cur_wp->command == (int)MAV_CMD_NAV_LOITER_TIME) {
- if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
- time_elapsed = true;
- }
- } else if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
+ if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
time_elapsed = true;
} else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) {
time_elapsed = true;
@@ -493,7 +487,7 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio
// mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
// }
- check_waypoints_reached(now, global_position , local_position);
+ check_waypoints_reached(now, global_position, local_position);
return OK;
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-24 00:49:20 +0000