void communication_send_remote_control(void)
{
	if (global_data.param[PARAM_SEND_SLOT_REMOTE_CONTROL] == 1)
	{
		mavlink_msg_rc_channels_raw_send(global_data.param[PARAM_SEND_DEBUGCHAN],
				radio_control_get_channel_raw(1),
				radio_control_get_channel_raw(2),
				radio_control_get_channel_raw(3),
				radio_control_get_channel_raw(4),
				radio_control_get_channel_raw(5),
				radio_control_get_channel_raw(6),
				radio_control_get_channel_raw(7),
				radio_control_get_channel_raw(8),
				((radio_control_status() > 0) ? 255 : 0));
				// Should be global_data.rc_rssi in the future

//		rc_to_255(1),
//		rc_to_255(2),
//		rc_to_255(3),
//		rc_to_255(4),
//		rc_to_255(5),
//		rc_to_255(6),
//		rc_to_255(7),
//		rc_to_255(8),
	}
}
Пример #2
0
/*
  send RC_CHANNELS_RAW, and RC_CHANNELS messages
 */
void GCS_MAVLINK::send_radio_in(uint8_t receiver_rssi)
{
    uint32_t now = hal.scheduler->millis();

    uint16_t values[8];
    memset(values, 0, sizeof(values));
    hal.rcin->read(values, 8);

    mavlink_msg_rc_channels_raw_send(
        chan,
        now,
        0, // port
        values[0],
        values[1],
        values[2],
        values[3],
        values[4],
        values[5],
        values[6],
        values[7],
        receiver_rssi);

#if HAL_CPU_CLASS > HAL_CPU_CLASS_16
    if (hal.rcin->num_channels() > 8 && 
        comm_get_txspace(chan) >= MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
        mavlink_msg_rc_channels_send(
            chan,
            now,
            hal.rcin->num_channels(),
            hal.rcin->read(CH_1),
            hal.rcin->read(CH_2),
            hal.rcin->read(CH_3),
            hal.rcin->read(CH_4),
            hal.rcin->read(CH_5),
            hal.rcin->read(CH_6),
            hal.rcin->read(CH_7),
            hal.rcin->read(CH_8),
            hal.rcin->read(CH_9),
            hal.rcin->read(CH_10),
            hal.rcin->read(CH_11),
            hal.rcin->read(CH_12),
            hal.rcin->read(CH_13),
            hal.rcin->read(CH_14),
            hal.rcin->read(CH_15),
            hal.rcin->read(CH_16),
            hal.rcin->read(CH_17),
            hal.rcin->read(CH_18),
            receiver_rssi);        
    }
#endif
}
Пример #3
0
	void send(const hrt_abstime t)
	{
		struct rc_input_values rc;

		if (rc_sub->update(&rc_time, &rc)) {
			const unsigned port_width = 8;

			// Deprecated message (but still needed for compatibility!)
			for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) {
				/* Channels are sent in MAVLink main loop at a fixed interval */
				mavlink_msg_rc_channels_raw_send(_channel,
								 rc.timestamp_publication / 1000,
								 i,
								 (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX,
								 (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX,
								 (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX,
								 (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX,
								 (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX,
								 (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX,
								 (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX,
								 (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX,
								 rc.rssi);
			}

			// New message
			mavlink_msg_rc_channels_send(_channel,
					rc.timestamp_publication / 1000,
					rc.channel_count,
					((rc.channel_count > 0) ? rc.values[0] : UINT16_MAX),
					((rc.channel_count > 1) ? rc.values[1] : UINT16_MAX),
					((rc.channel_count > 2) ? rc.values[2] : UINT16_MAX),
					((rc.channel_count > 3) ? rc.values[3] : UINT16_MAX),
					((rc.channel_count > 4) ? rc.values[4] : UINT16_MAX),
					((rc.channel_count > 5) ? rc.values[5] : UINT16_MAX),
					((rc.channel_count > 6) ? rc.values[6] : UINT16_MAX),
					((rc.channel_count > 7) ? rc.values[7] : UINT16_MAX),
					((rc.channel_count > 8) ? rc.values[8] : UINT16_MAX),
					((rc.channel_count > 9) ? rc.values[9] : UINT16_MAX),
					((rc.channel_count > 10) ? rc.values[10] : UINT16_MAX),
					((rc.channel_count > 11) ? rc.values[11] : UINT16_MAX),
					((rc.channel_count > 12) ? rc.values[12] : UINT16_MAX),
					((rc.channel_count > 13) ? rc.values[13] : UINT16_MAX),
					((rc.channel_count > 14) ? rc.values[14] : UINT16_MAX),
					((rc.channel_count > 15) ? rc.values[15] : UINT16_MAX),
					((rc.channel_count > 16) ? rc.values[16] : UINT16_MAX),
					((rc.channel_count > 17) ? rc.values[17] : UINT16_MAX),
					rc.rssi);
		}
	}
Пример #4
0
void
l_input_rc(const 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_raw.timestamp / 1000,
						 0,
						 (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);
}
Пример #5
0
	void send(const hrt_abstime t)
	{
		if (rc_sub->update(t)) {
			const unsigned port_width = 8;

			for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) {
				/* Channels are sent in MAVLink main loop at a fixed interval */
				mavlink_msg_rc_channels_raw_send(_channel,
								 rc->timestamp_publication / 1000,
								 i,
								 (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX,
								 (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX,
								 (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX,
								 (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX,
								 (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX,
								 (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX,
								 (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX,
								 (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX,
								 rc->rssi);
			}
		}
	}
void MavlinkComm::sendMessage(uint8_t id, uint32_t param) {
    //_board->debug->printf_P(PSTR("send message\n"));

    // if number of channels exceeded return
    if (_channel == MAVLINK_COMM_3)
        return;

    uint64_t timeStamp = micros();

    switch (id) {

    case MAVLINK_MSG_ID_HEARTBEAT: {
        mavlink_msg_heartbeat_send(_channel, _board->getVehicle(),
                                   MAV_AUTOPILOT_ARDUPILOTMEGA);
        break;
    }

    case MAVLINK_MSG_ID_ATTITUDE: {
        mavlink_msg_attitude_send(_channel, timeStamp,
                                  _navigator->getRoll(), _navigator->getPitch(),
                                  _navigator->getYaw(), _navigator->getRollRate(),
                                  _navigator->getPitchRate(), _navigator->getYawRate());
        break;
    }

    case MAVLINK_MSG_ID_GLOBAL_POSITION: {
        mavlink_msg_global_position_send(_channel, timeStamp,
                                         _navigator->getLat() * rad2Deg,
                                         _navigator->getLon() * rad2Deg, _navigator->getAlt(),
                                         _navigator->getVN(), _navigator->getVE(),
                                         _navigator->getVD());
        break;
    }

    case MAVLINK_MSG_ID_LOCAL_POSITION: {
        mavlink_msg_local_position_send(_channel, timeStamp,
                _navigator->getPN(),_navigator->getPE(), _navigator->getPD(),
                _navigator->getVN(), _navigator->getVE(), _navigator->getVD());
        break;
    }

    case MAVLINK_MSG_ID_GPS_RAW: {
        mavlink_msg_gps_raw_send(_channel, timeStamp, _board->gps->status(),
                                 _board->gps->latitude/1.0e7,
                                 _board->gps->longitude/1.0e7, _board->gps->altitude/100.0, 0, 0,
                                 _board->gps->ground_speed/100.0,
                                 _board->gps->ground_course/10.0);
        break;
    }

    case MAVLINK_MSG_ID_GPS_RAW_INT: {
        mavlink_msg_gps_raw_send(_channel, timeStamp, _board->gps->status(),
                                 _board->gps->latitude,
                                 _board->gps->longitude, _board->gps->altitude*10.0, 0, 0,
                                 _board->gps->ground_speed/100.0,
                                 _board->gps->ground_course/10.0);
        break;
     }

    case MAVLINK_MSG_ID_SCALED_IMU: {
        int16_t xmag, ymag, zmag;
        xmag = ymag = zmag = 0;
        if (_board->compass) {
            // XXX THIS IS NOT SCALED
            xmag = _board->compass->mag_x;
            ymag = _board->compass->mag_y;
            zmag = _board->compass->mag_z;
        }
        mavlink_msg_scaled_imu_send(_channel, timeStamp,
            _navigator->getXAccel()*1e3,
            _navigator->getYAccel()*1e3,
            _navigator->getZAccel()*1e3,
            _navigator->getRollRate()*1e3,
            _navigator->getPitchRate()*1e3,
            _navigator->getYawRate()*1e3,
            xmag, ymag, zmag);
        break;
    }

    case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: {
        int16_t ch[8];
        for (int i = 0; i < 8; i++)
            ch[i] = 0;
        for (uint8_t i = 0; i < 8 && i < _board->rc.getSize(); i++) {
            ch[i] = 10000 * _board->rc[i]->getPosition();
            //_board->debug->printf_P(PSTR("ch: %d position: %d\n"),i,ch[i]);
        }
        mavlink_msg_rc_channels_scaled_send(_channel, ch[0], ch[1], ch[2],
                                            ch[3], ch[4], ch[5], ch[6], ch[7], 255);
        break;
    }

    case MAVLINK_MSG_ID_RC_CHANNELS_RAW: {
        int16_t ch[8];
        for (int i = 0; i < 8; i++)
            ch[i] = 0;
        for (uint8_t i = 0; i < 8 && i < _board->rc.getSize(); i++)
            ch[i] = _board->rc[i]->getPwm();
        mavlink_msg_rc_channels_raw_send(_channel, ch[0], ch[1], ch[2],
                                         ch[3], ch[4], ch[5], ch[6], ch[7], 255);
        break;
    }

    case MAVLINK_MSG_ID_SYS_STATUS: {

        uint16_t batteryVoltage = 0; // (milli volts)
        uint16_t batteryPercentage = 1000; // times 10
        if (_board->batteryMonitor) {
            batteryPercentage = _board->batteryMonitor->getPercentage()*10;
            batteryVoltage = _board->batteryMonitor->getVoltage()*1000;
        }
        mavlink_msg_sys_status_send(_channel, _controller->getMode(),
                                    _guide->getMode(), _controller->getState(), _board->load * 10,
                                    batteryVoltage, batteryPercentage, _packetDrops);
        break;
    }

    case MAVLINK_MSG_ID_WAYPOINT_ACK: {
        sendText(SEVERITY_LOW, PSTR("waypoint ack"));
        //mavlink_waypoint_ack_t packet;
        uint8_t type = 0; // ok (0), error(1)
        mavlink_msg_waypoint_ack_send(_channel, _cmdDestSysId,
                                      _cmdDestCompId, type);

        // turn off waypoint send
        _receivingCmds = false;
        break;
    }

    case MAVLINK_MSG_ID_WAYPOINT_CURRENT: {
        mavlink_msg_waypoint_current_send(_channel,
                                          _guide->getCurrentIndex());
        break;
    }

    default: {
        char msg[50];
        sprintf(msg, "autopilot sending unknown command with id: %d", id);
        sendText(SEVERITY_HIGH, msg);
    }

    } // switch
} // send message