aboutsummaryrefslogtreecommitdiff
path: root/apps/mavlink
diff options
context:
space:
mode:
Diffstat (limited to 'apps/mavlink')
-rw-r--r--apps/mavlink/mavlink.c1
-rw-r--r--apps/mavlink/mavlink_parameters.c78
-rw-r--r--apps/mavlink/mavlink_receiver.c21
-rw-r--r--apps/mavlink/orb_listener.c45
-rw-r--r--apps/mavlink/orb_topics.h2
5 files changed, 58 insertions, 89 deletions
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index 81b907bcd..77034914f 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -745,6 +745,7 @@ int mavlink_main(int argc, char *argv[])
thread_should_exit = true;
while (thread_running) {
usleep(200000);
+ printf(".");
}
warnx("terminated.");
exit(0);
diff --git a/apps/mavlink/mavlink_parameters.c b/apps/mavlink/mavlink_parameters.c
index 7cb1582b4..9d9b9914a 100644
--- a/apps/mavlink/mavlink_parameters.c
+++ b/apps/mavlink/mavlink_parameters.c
@@ -72,20 +72,6 @@ static unsigned int mavlink_param_queue_index = 0;
*/
void mavlink_pm_callback(void *arg, param_t param);
-/**
- * Save parameters to EEPROM.
- *
- * Stores the parameters to /eeprom/parameters
- */
-static int mavlink_pm_save_eeprom(void);
-
-/**
- * Load parameters from EEPROM.
- *
- * Loads the parameters from /eeprom/parameters
- */
-static int mavlink_pm_load_eeprom(void);
-
void mavlink_pm_callback(void *arg, param_t param)
{
mavlink_pm_send_param(param);
@@ -232,69 +218,5 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess
}
} break;
-
- // case MAVLINK_MSG_ID_COMMAND_LONG: {
- // mavlink_command_long_t cmd_mavlink;
- // mavlink_msg_command_long_decode(msg, &cmd_mavlink);
-
- // uint8_t result = MAV_RESULT_UNSUPPORTED;
-
- // if (cmd_mavlink.target_system == mavlink_system.sysid &&
- // ((cmd_mavlink.target_component == mavlink_system.compid) ||(cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
-
- // // XXX move this to LOW PRIO THREAD of commander app
-
- // /* preflight parameter load / store */
- // if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_STORAGE) {
- // /* Read all parameters from EEPROM to RAM */
-
- // if (((int)(cmd_mavlink.param1)) == 0) {
-
- // /* read all parameters from EEPROM to RAM */
- // int read_ret = param_load_default();
- // if (read_ret == OK) {
- // //printf("[mavlink pm] Loaded EEPROM params in RAM\n");
- // mavlink_missionlib_send_gcs_string("[pm] OK loading %s", param_get_default_file());
- // result = MAV_RESULT_ACCEPTED;
- // } else if (read_ret == 1) {
- // mavlink_missionlib_send_gcs_string("[pm] OK no changes %s", param_get_default_file());
- // result = MAV_RESULT_ACCEPTED;
- // } else {
- // if (read_ret < -1) {
- // mavlink_missionlib_send_gcs_string("[pm] ERR loading %s", param_get_default_file());
- // } else {
- // mavlink_missionlib_send_gcs_string("[pm] ERR no file %s", param_get_default_file());
- // }
- // result = MAV_RESULT_FAILED;
- // }
-
- // } else if (((int)(cmd_mavlink.param1)) == 1) {
-
- // /* write all parameters from RAM to EEPROM */
- // int write_ret = param_save_default();
- // if (write_ret == OK) {
- // mavlink_missionlib_send_gcs_string("[pm] OK saved %s", param_get_default_file());
- // result = MAV_RESULT_ACCEPTED;
-
- // } else {
- // if (write_ret < -1) {
- // mavlink_missionlib_send_gcs_string("[pm] ERR writing %s", param_get_default_file());
- // } else {
- // mavlink_missionlib_send_gcs_string("[pm] ERR writing %s", param_get_default_file());
- // }
- // result = MAV_RESULT_FAILED;
- // }
-
- // } else {
- // //fprintf(stderr, "[mavlink pm] refusing unsupported storage request\n");
- // mavlink_missionlib_send_gcs_string("[pm] refusing unsupp. STOR request");
- // result = MAV_RESULT_UNSUPPORTED;
- // }
- // }
- // }
-
- // /* send back command result */
- // //mavlink_msg_command_ack_send(chan, cmd.command, result);
- // } break;
}
}
diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c
index dd011aeed..fb59d07a1 100644
--- a/apps/mavlink/mavlink_receiver.c
+++ b/apps/mavlink/mavlink_receiver.c
@@ -298,6 +298,26 @@ handle_message(mavlink_message_t *msg)
mavlink_hil_state_t hil_state;
mavlink_msg_hil_state_decode(msg, &hil_state);
+ /* Calculate Rotation Matrix */
+ //TODO: better clarification which app does this, atm we have a ekf for quadrotors which does this, but there is no such thing if fly in fixed wing mode
+
+ if(mavlink_system.type == MAV_TYPE_FIXED_WING) {
+ //TODO: assuming low pitch and roll values for now
+ hil_attitude.R[0][0] = cosf(hil_state.yaw);
+ hil_attitude.R[0][1] = sinf(hil_state.yaw);
+ hil_attitude.R[0][2] = 0.0f;
+
+ hil_attitude.R[1][0] = -sinf(hil_state.yaw);
+ hil_attitude.R[1][1] = cosf(hil_state.yaw);
+ hil_attitude.R[1][2] = 0.0f;
+
+ hil_attitude.R[2][0] = 0.0f;
+ hil_attitude.R[2][1] = 0.0f;
+ hil_attitude.R[2][2] = 1.0f;
+
+ hil_attitude.R_valid = true;
+ }
+
hil_global_pos.lat = hil_state.lat;
hil_global_pos.lon = hil_state.lon;
hil_global_pos.alt = hil_state.alt / 1000.0f;
@@ -305,6 +325,7 @@ handle_message(mavlink_message_t *msg)
hil_global_pos.vy = hil_state.vy / 100.0f;
hil_global_pos.vz = hil_state.vz / 100.0f;
+
/* set timestamp and notify processes (broadcast) */
hil_global_pos.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c
index a067460d8..b0aa401fc 100644
--- a/apps/mavlink/orb_listener.c
+++ b/apps/mavlink/orb_listener.c
@@ -114,6 +114,7 @@ static void l_manual_control_setpoint(struct listener *l);
static void l_vehicle_attitude_controls(struct listener *l);
static void l_debug_key_value(struct listener *l);
static void l_optical_flow(struct listener *l);
+static void l_vehicle_rates_setpoint(struct listener *l);
struct listener listeners[] = {
{l_sensor_combined, &mavlink_subs.sensor_sub, 0},
@@ -136,6 +137,7 @@ struct listener listeners[] = {
{l_vehicle_attitude_controls, &mavlink_subs.actuators_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},
};
static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
@@ -409,6 +411,23 @@ l_attitude_setpoint(struct listener *l)
}
void
+l_vehicle_rates_setpoint(struct listener *l)
+{
+ struct vehicle_rates_setpoint_s rates_sp;
+
+ /* copy local position data into local buffer */
+ orb_copy(ORB_ID(vehicle_rates_setpoint), mavlink_subs.rates_setpoint_sub, &rates_sp);
+
+ if (gcs_link)
+ mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(MAVLINK_COMM_0,
+ rates_sp.timestamp/1000,
+ rates_sp.roll,
+ rates_sp.pitch,
+ rates_sp.yaw,
+ rates_sp.thrust);
+}
+
+void
l_actuator_outputs(struct listener *l)
{
struct actuator_outputs_s act_outputs;
@@ -547,28 +566,28 @@ l_manual_control_setpoint(struct listener *l)
void
l_vehicle_attitude_controls(struct listener *l)
{
- struct actuator_controls_s actuators;
+ struct actuator_controls_effective_s actuators;
- orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators);
+ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators);
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.control[0]);
+ "eff ctrl0 ",
+ actuators.control_effective[0]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
- "ctrl1 ",
- actuators.control[1]);
+ "eff ctrl1 ",
+ actuators.control_effective[1]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
- "ctrl2 ",
- actuators.control[2]);
+ "eff ctrl2 ",
+ actuators.control_effective[2]);
mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
last_sensor_timestamp / 1000,
- "ctrl3 ",
- actuators.control[3]);
+ "eff ctrl3 ",
+ actuators.control_effective[3]);
}
}
@@ -695,6 +714,10 @@ uorb_receive_start(void)
mavlink_subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
orb_set_interval(mavlink_subs.spa_sub, 2000); /* 0.5 Hz updates */
+ /* --- RATES SETPOINT VALUE --- */
+ mavlink_subs.rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
+ orb_set_interval(mavlink_subs.rates_setpoint_sub, 2000); /* 0.5 Hz updates */
+
/* --- ACTUATOR OUTPUTS --- */
mavlink_subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0));
mavlink_subs.act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1));
@@ -716,7 +739,7 @@ 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);
+ mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */
/* --- DEBUG VALUE OUTPUT --- */
diff --git a/apps/mavlink/orb_topics.h b/apps/mavlink/orb_topics.h
index 61e664401..4650c4991 100644
--- a/apps/mavlink/orb_topics.h
+++ b/apps/mavlink/orb_topics.h
@@ -52,6 +52,7 @@
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_global_position_setpoint.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_controls_effective.h>
@@ -79,6 +80,7 @@ struct mavlink_subscriptions {
int debug_key_value;
int input_rc_sub;
int optical_flow;
+ int rates_setpoint_sub;
};
extern struct mavlink_subscriptions mavlink_subs;