Example #1
0
void
l_vehicle_attitude_controls(struct listener *l)
{
	struct actuator_controls_s actuators;

	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, 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]);
		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
						   last_sensor_timestamp / 1000,
						   "ctrl1       ",
						   actuators.control[1]);
		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
						   last_sensor_timestamp / 1000,
						   "ctrl2       ",
						   actuators.control[2]);
		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
						   last_sensor_timestamp / 1000,
						   "ctrl3       ",
						   actuators.control[3]);
	}

	/* Only send in HIL mode */
	if (mavlink_hil_enabled) {

		/* 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);

		/* HIL message as per MAVLink spec */
		mavlink_msg_hil_controls_send(chan,
			hrt_absolute_time(),
			actuators.control[0],
			actuators.control[1],
			actuators.control[2],
			actuators.control[3],
			0,
			0,
			0,
			0,
			mavlink_mode,
			0);
	}
}
Example #2
0
void
l_actuator_outputs(const struct listener *l)
{
	struct actuator_outputs_s act_outputs;

	orb_id_t ids[] = {
		ORB_ID(actuator_outputs_0),
		ORB_ID(actuator_outputs_1),
		ORB_ID(actuator_outputs_2),
		ORB_ID(actuator_outputs_3)
	};

	/* copy actuator data into local buffer */
	orb_copy(ids[l->arg], *l->subp, &act_outputs);

	if (gcs_link) {
		mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000,
						  l->arg /* port number */,
						  act_outputs.output[0],
						  act_outputs.output[1],
						  act_outputs.output[2],
						  act_outputs.output[3],
						  act_outputs.output[4],
						  act_outputs.output[5],
						  act_outputs.output[6],
						  act_outputs.output[7]);

		/* only send in HIL mode */
		if (mavlink_hil_enabled && armed.armed) {

			/* 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);

			/* HIL message as per MAVLink spec */

			/* scale / assign outputs depending on system type */

			if (mavlink_system.type == MAV_TYPE_QUADROTOR) {
				mavlink_msg_hil_controls_send(chan,
							      hrt_absolute_time(),
							      ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
							      ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
							      ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
							      ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
							      -1,
							      -1,
							      -1,
							      -1,
							      mavlink_mode,
							      0);

			} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
				mavlink_msg_hil_controls_send(chan,
							      hrt_absolute_time(),
							      ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
							      ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
							      ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
							      ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
							      ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
							      ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
							      -1,
							      -1,
							      mavlink_mode,
							      0);

			} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
				mavlink_msg_hil_controls_send(chan,
							      hrt_absolute_time(),
							      ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
							      ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
							      ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
							      ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
							      ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
							      ((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,
							      0);

			} else {
				mavlink_msg_hil_controls_send(chan,
							      hrt_absolute_time(),
							      (act_outputs.output[0] - 1500.0f) / 500.0f,
							      (act_outputs.output[1] - 1500.0f) / 500.0f,
							      (act_outputs.output[2] - 1500.0f) / 500.0f,
							      (act_outputs.output[3] - 1000.0f) / 1000.0f,
							      (act_outputs.output[4] - 1500.0f) / 500.0f,
							      (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,
							      0);
			}
		}
	}
}
Example #3
0
	void send(const hrt_abstime t)
	{
		struct vehicle_status_s status;
		struct position_setpoint_triplet_s pos_sp_triplet;
		struct actuator_outputs_s act;

		bool updated = act_sub->update(&act_time, &act);
		updated |= pos_sp_triplet_sub->update(&pos_sp_triplet_time, &pos_sp_triplet);
		updated |= status_sub->update(&status_time, &status);

		if (updated && (status.arming_state == ARMING_STATE_ARMED)) {
			/* translate the current syste state to mavlink state and mode */
			uint8_t mavlink_state;
			uint8_t mavlink_base_mode;
			uint32_t mavlink_custom_mode;
			get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);

			if (mavlink_system.type == MAV_TYPE_QUADROTOR ||
				mavlink_system.type == MAV_TYPE_HEXAROTOR ||
				mavlink_system.type == MAV_TYPE_OCTOROTOR) {
				/* set number of valid outputs depending on vehicle type */
				unsigned n;

				switch (mavlink_system.type) {
				case MAV_TYPE_QUADROTOR:
					n = 4;
					break;

				case MAV_TYPE_HEXAROTOR:
					n = 6;
					break;

				default:
					n = 8;
					break;
				}

				/* scale / assign outputs depending on system type */
				float out[8];

				for (unsigned i = 0; i < 8; i++) {
					if (i < n) {
						if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
							/* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */
							out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);

						} else {
							/* send 0 when disarmed */
							out[i] = 0.0f;
						}

					} else {
						out[i] = -1.0f;
					}
				}

				mavlink_msg_hil_controls_send(_channel,
							      hrt_absolute_time(),
							      out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
							      mavlink_base_mode,
							      0);
			} else {

				/* fixed wing: scale all channels except throttle -1 .. 1
				 * because we know that we set the mixers up this way
				 */

				float out[8];

				const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;

				for (unsigned i = 0; i < 8; i++) {
					if (i != 3) {
						/* scale fake PWM out 900..2100 us to -1..+1 for normal channels */
						out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);

					} else {

						/* scale fake PWM out 900..2100 us to 0..1 for throttle */
						out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
					}

				}

				mavlink_msg_hil_controls_send(_channel,
							      hrt_absolute_time(),
							      out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7],
							      mavlink_base_mode,
							      0);
			}
		}
	}
Example #4
0
void
l_actuator_outputs(struct listener *l)
{
    struct actuator_outputs_s act_outputs;

    orb_id_t ids[] = {
        ORB_ID(actuator_outputs_0),
        ORB_ID(actuator_outputs_1),
        ORB_ID(actuator_outputs_2),
        ORB_ID(actuator_outputs_3)
    };

    /* copy actuator data into local buffer */
    orb_copy(ids[l->arg], *l->subp, &act_outputs);

    if (gcs_link) {
        mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000,
                                          l->arg /* port number */,
                                          act_outputs.output[0],
                                          act_outputs.output[1],
                                          act_outputs.output[2],
                                          act_outputs.output[3],
                                          act_outputs.output[4],
                                          act_outputs.output[5],
                                          act_outputs.output[6],
                                          act_outputs.output[7]);

        /* only send in HIL mode */
        if (mavlink_hil_enabled) {

            /* 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);

            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) / 1000.0f;
                throttle = (act_outputs.output[3] - 1000.0f) / 1000.0f;
            } else {
                /* only three outputs, put throttle on position 4 / index 3 */
                rudder = 0;
                throttle = (act_outputs.output[2] - 1000.0f) / 1000.0f;
            }

            /* HIL message as per MAVLink spec */
            mavlink_msg_hil_controls_send(chan,
                                          hrt_absolute_time(),
                                          (act_outputs.output[0] - 1500.0f) / 1000.0f,
                                          (act_outputs.output[1] - 1500.0f) / 1000.0f,
                                          rudder,
                                          throttle,
                                          (act_outputs.output[4] - 1500.0f) / 1000.0f,
                                          (act_outputs.output[5] - 1500.0f) / 1000.0f,
                                          (act_outputs.output[6] - 1500.0f) / 1000.0f,
                                          (act_outputs.output[7] - 1500.0f) / 1000.0f,
                                          mavlink_mode,
                                          0);
        }
    }
}