void communication_send_attitude_position(uint64_t loop_start_time)
{
// ATTITUDE at 20 Hz
if (global_data.param[PARAM_SEND_SLOT_ATTITUDE] == 1)
{
// Send attitude over both UART ports
mavlink_msg_attitude_send(global_data.param[PARAM_SEND_DEBUGCHAN],
sys_time_clock_get_unix_offset() + loop_start_time,
global_data.attitude.x, global_data.attitude.y,
global_data.attitude.z, global_data.gyros_si.x,
global_data.gyros_si.y, global_data.gyros_si.z);
/* mavlink_msg_attitude_send(MAVLINK_COMM_1,
sys_time_clock_get_unix_offset() + loop_start_time,
global_data.attitude.x, global_data.attitude.y,
global_data.attitude.z, global_data.gyros_si.x,
global_data.gyros_si.y, global_data.gyros_si.z);*/
}
if (global_data.param[PARAM_SEND_SLOT_DEBUG_5] == 1)
{
// Send current position and speed
mavlink_msg_local_position_send(global_data.param[PARAM_SEND_DEBUGCHAN], sys_time_clock_get_unix_offset() + loop_start_time,
global_data.position.x, global_data.position.y,
global_data.position.z, global_data.velocity.x,
global_data.velocity.y, global_data.velocity.z);
}
}
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
