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