aboutsummaryrefslogtreecommitdiff
path: root/apps/mavlink
diff options
context:
space:
mode:
Diffstat (limited to 'apps/mavlink')
-rw-r--r--apps/mavlink/mavlink.c35
-rw-r--r--apps/mavlink/mavlink_parameters.c4
-rw-r--r--apps/mavlink/mavlink_receiver.c25
-rw-r--r--apps/mavlink/orb_listener.c45
4 files changed, 75 insertions, 34 deletions
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index 81b907bcd..ccc9d6d7d 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -189,12 +189,32 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
*mavlink_mode = 0;
/* set mode flags independent of system state */
+
+ /* HIL */
+ if (v_status.flag_hil_enabled) {
+ *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
+ }
+
+ /* manual input */
if (v_status.flag_control_manual_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
}
- if (v_status.flag_hil_enabled) {
- *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
+ /* 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 */
@@ -225,17 +245,14 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
case SYSTEM_STATE_MANUAL:
*mavlink_state = MAV_STATE_ACTIVE;
- *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
break;
case SYSTEM_STATE_STABILIZED:
*mavlink_state = MAV_STATE_ACTIVE;
- *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
break;
case SYSTEM_STATE_AUTO:
*mavlink_state = MAV_STATE_ACTIVE;
- *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
break;
case SYSTEM_STATE_MISSION_ABORT:
@@ -470,14 +487,15 @@ mavlink_message_t* mavlink_get_channel_buffer(uint8_t channel)
void mavlink_update_system(void)
{
static bool initialized = false;
- param_t param_system_id;
- param_t param_component_id;
- param_t param_system_type;
+ static param_t param_system_id;
+ static param_t param_component_id;
+ static param_t param_system_type;
if (!initialized) {
param_system_id = param_find("MAV_SYS_ID");
param_component_id = param_find("MAV_COMP_ID");
param_system_type = param_find("MAV_TYPE");
+ initialized = true;
}
/* update system and component id */
@@ -745,6 +763,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 9d9b9914a..f7638550d 100644
--- a/apps/mavlink/mavlink_parameters.c
+++ b/apps/mavlink/mavlink_parameters.c
@@ -143,7 +143,9 @@ int mavlink_pm_send_param(param_t param)
*/
int ret;
- if ((ret = param_get(param, &val_buf)) != OK) return ret;
+ if ((ret = param_get(param, &val_buf)) != OK) {
+ return ret;
+ }
mavlink_msg_param_value_pack_chan(mavlink_system.sysid,
mavlink_system.compid,
diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c
index dd011aeed..58761e89c 100644
--- a/apps/mavlink/mavlink_receiver.c
+++ b/apps/mavlink/mavlink_receiver.c
@@ -261,8 +261,8 @@ handle_message(mavlink_message_t *msg)
offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid-1] / (float)INT16_MAX;
offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid-1] / (float)INT16_MAX;
- offboard_control_sp.p3= (float)quad_motors_setpoint.yaw[mavlink_system.sysid-1] / (float)INT16_MAX;
- offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1]/(float)UINT16_MAX;
+ offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid-1] / (float)INT16_MAX;
+ offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1] / (float)UINT16_MAX;
if (quad_motors_setpoint.thrust[mavlink_system.sysid-1] == 0) {
ml_armed = false;
@@ -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 e6ec77bf6..40e52a500 100644
--- a/apps/mavlink/orb_listener.c
+++ b/apps/mavlink/orb_listener.c
@@ -455,7 +455,7 @@ l_actuator_outputs(struct listener *l)
act_outputs.output[7]);
/* only send in HIL mode */
- if (mavlink_hil_enabled) {
+ if (mavlink_hil_enabled && armed.armed) {
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
@@ -506,20 +506,19 @@ l_actuator_outputs(struct listener *l)
mavlink_mode,
0);
} else {
- float rudder, throttle;
- /* SCALING: PWM min: 900, PWM max: 2100, center: 1500 */
+ /*
+ * Catch the case where no rudder is in use and throttle is not
+ * on output four
+ */
+ float rudder, throttle;
- // XXX very ugly, needs rework
- if (isfinite(act_outputs.output[3])
- && act_outputs.output[3] > 800 && act_outputs.output[3] < 2200) {
- /* throttle is fourth output */
- rudder = (act_outputs.output[2] - 1500.0f) / 600.0f;
- throttle = (((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f);
+ if (act_outputs.noutputs < 4) {
+ rudder = 0.0f;
+ throttle = (act_outputs.output[2] - 900.0f) / 1200.0f;
} else {
- /* only three outputs, put throttle on position 4 / index 3 */
- rudder = 0;
- throttle = (((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f);
+ rudder = (act_outputs.output[2] - 1500.0f) / 600.0f;
+ throttle = (act_outputs.output[3] - 900.0f) / 1200.0f;
}
mavlink_msg_hil_controls_send(chan,
@@ -566,28 +565,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]);
}
}
@@ -739,7 +738,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 --- */
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-13 18:44:40 +0000