// locks onto a particular target sysid and sets it's position data stream to at least 1hz
void Tracker::mavlink_check_target(const mavlink_message_t* msg)
{
// exit immediately if the target has already been set
if (target_set) {
return;
}
// decode
mavlink_heartbeat_t packet;
mavlink_msg_heartbeat_decode(msg, &packet);
// exit immediately if this is not a vehicle we would track
if ((packet.type == MAV_TYPE_ANTENNA_TRACKER) ||
(packet.type == MAV_TYPE_GCS) ||
(packet.type == MAV_TYPE_ONBOARD_CONTROLLER) ||
(packet.type == MAV_TYPE_GIMBAL)) {
return;
}
// set our sysid to the target, this ensures we lock onto a single vehicle
if (g.sysid_target == 0) {
g.sysid_target = msg->sysid;
}
// send data stream request to target on all channels
// Note: this doesn't check success for all sends meaning it's not guaranteed the vehicle's positions will be sent at 1hz
for (uint8_t i=0; i < num_gcs; i++) {
if (gcs[i].initialised) {
// request position
if (HAVE_PAYLOAD_SPACE((mavlink_channel_t)i, DATA_STREAM)) {
mavlink_msg_request_data_stream_send(
(mavlink_channel_t)i,
msg->sysid,
msg->compid,
MAV_DATA_STREAM_POSITION,
g.mavlink_update_rate,
1); // start streaming
}
// request air pressure
if (HAVE_PAYLOAD_SPACE((mavlink_channel_t)i, DATA_STREAM)) {
mavlink_msg_request_data_stream_send(
(mavlink_channel_t)i,
msg->sysid,
msg->compid,
MAV_DATA_STREAM_RAW_SENSORS,
g.mavlink_update_rate,
1); // start streaming
}
}
}
// flag target has been set
target_set = true;
}
/**
* @brief -- parse mavlink_msg after the char is processed
* @param msg -- the msg to be parsed
* @return -- NULL
*/
void UAS_comm::parseGcsMessage(mavlink_message_t* pMsg){
switch (pMsg->msgid) {
case MAVLINK_MSG_ID_HEARTBEAT:
mavlink_heartbeat_t heartbeat;
mavlink_msg_heartbeat_decode(pMsg, &heartbeat);
bypassMessage(chan_apm, pMsg);
break;
default:
if (isApmOpen()) {
bypassMessage(chan_apm, pMsg); // pass the msg to apm
}
break;
} // end switch
}
void MavLinkInterpreter::ProcessHeartbeat()
{
_hb_count++;
mavlink_heartbeat_t hb;
mavlink_msg_heartbeat_decode(&_msg, &hb);
_m->BaseMode = (hb.base_mode & 0x80) > 7;
_m->CustomMode = hb.custom_mode;
_m->MavlinkVersion = hb.mavlink_version;
#ifdef DEBUG_APM_HEARTBEAT
debugSerial.printf("%d\tMAVLINK_MSG_ID_SYS_STATUS: heartbeats_rcvd: %d, status: %d, autopilot: %d, base_mode: %d, custom: %d\r\n", millis(),
_hb_count, hb.system_status, hb.autopilot, _m->BaseMode, _m->CustomMode);
#endif
_mavLinkConnectedTimer = millis() + 1500;
}
/**
* @brief -- parse mavlink_msg after the char is processed
* @param msg -- the msg to be parsed
* @return -- NULL
*/
void UAS_comm::parseApmMessage(mavlink_message_t* pMsg){
switch (pMsg->msgid){
case MAVLINK_MSG_ID_HEARTBEAT:
mavlink_heartbeat_t heartbeat;
mavlink_msg_heartbeat_decode(pMsg, &heartbeat);
bypassMessage(chan_gcs, pMsg);
break;
case MAVLINK_MSG_ID_ATTITUDE:
mavlink_attitude_t attitude;
mavlink_msg_attitude_decode(pMsg, &attitude);
updateAttitude(attitude.roll,
attitude.pitch,
attitude.yaw,
attitude.rollspeed,
attitude.pitchspeed,
attitude.yawspeed);
bypassMessage(chan_gcs, pMsg);
break;
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
mavlink_global_position_int_t global_position;
mavlink_msg_global_position_int_decode(pMsg, &global_position);
updateGlobalPosition(global_position.lat,
global_position.lon,
global_position.alt,
global_position.relative_alt,
global_position.vx,
global_position.vy,
global_position.vz,
global_position.hdg);
bypassMessage(chan_gcs, pMsg);
break;
default:
if (isGcsOpen()) {
bypassMessage(chan_gcs, pMsg); // pass the message to gcs
}
break;
} // end switch
}
// ------------------------------------------------------------------------------
// Read Messages
// ------------------------------------------------------------------------------
void
Autopilot_Interface::
read_messages()
{
int success = 0; // receive success flag
bool received_all = false; // receive only one message
Time_Stamps this_timestamps;
int err_counter = 0;
struct timespec diff_r;
struct timespec aft_read;
struct timespec bef_read;
struct timespec receive_time;
struct timespec longRelNanoSleep;
longRelNanoSleep.tv_sec = 0;
longRelNanoSleep.tv_nsec = 100000;
struct timespec RelNanoSleep;
RelNanoSleep.tv_sec = 0;
RelNanoSleep.tv_nsec = 10;
// Blocking wait for new data
// pthread_mutex_lock(&mut);
// if (blocked == 0 && writing_status)
// {
// blocked = 1;
// pthread_cond_wait(&cond,&mut);
// blocked = 0;
// }
// else
// {
// pthread_cond_signal(&cond);
// }
// clock_gettime(CLOCK_REALTIME,&bef_read);
// clock_gettime(CLOCK_REALTIME,&aft_read);
// timespec_sub(&diff_r,&aft_read,&bef_read);
// int64_t diff_r_us = diff_r.tv_sec*1e6 + diff_r.tv_nsec/1e3;
// printf("Read Time: %ld \n",diff_r_us);
//
// pthread_mutex_unlock(&mut);
mavlink_message_t message;
while ( (success != 1) && (!time_to_exit) )
{
// ----------------------------------------------------------------------
// READ ONE BYTE AT TIME UNTIL COMPLETE MESSAGE IS RECEIVED
// ----------------------------------------------------------------------
success = serial_port->read_message(message); //Have I obtained a complete message?
if ( success == -1) // Errors...retry...
{
err_counter++;
if (err_counter == 3) // To many void read
{
fprintf(stderr,"ERRORS!\n");
break;
}
}
}
err_counter = 0;
if (success == -1) return;
// ----------------------------------------------------------------------
// HANDLE MESSAGE
// ----------------------------------------------------------------------
//if( success )
// Store message sysid and compid.
// Note this doesn't handle multiple message sources.
current_messages.sysid = message.sysid;
current_messages.compid = message.compid;
// Handle Message ID
switch (message.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
//printf("MAVLINK_MSG_ID_HEARTBEAT\n");
mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));
base_mode = current_messages.heartbeat.base_mode;
mav_type = current_messages.heartbeat.type;
system_status = current_messages.heartbeat.system_status;
custom_mode = current_messages.heartbeat.custom_mode;
//printf("base_mode = %u\n",base_mode);
current_messages.time_stamps.heartbeat = get_time_usec();
this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
heartbeat_count++;
//printf("IMU Sensors Timestamp %u\n",current_messages.time_stamps.highres_imu/1000);
if ( (current_messages.time_stamps.heartbeat - read_heartbeat_old) > 10000000 )
{
printf("HEARTBEAT frequency : %d \n",heartbeat_count/10);
heartbeat_count = 0;
read_heartbeat_old = current_messages.time_stamps.heartbeat;
}
break;
}
case MAVLINK_MSG_ID_SYS_STATUS:
{
//printf("MAVLINK_MSG_ID_SYS_STATUS\n");
mavlink_msg_sys_status_decode(&message, &(current_messages.sys_status));
current_messages.time_stamps.sys_status = get_time_usec();
this_timestamps.sys_status = current_messages.time_stamps.sys_status;
break;
}
case MAVLINK_MSG_ID_BATTERY_STATUS:
{
//printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
mavlink_msg_battery_status_decode(&message, &(current_messages.battery_status));
current_messages.time_stamps.battery_status = get_time_usec();
this_timestamps.battery_status = current_messages.time_stamps.battery_status;
break;
}
case MAVLINK_MSG_ID_RADIO_STATUS:
{
//printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
mavlink_msg_radio_status_decode(&message, &(current_messages.radio_status));
current_messages.time_stamps.radio_status = get_time_usec();
this_timestamps.radio_status = current_messages.time_stamps.radio_status;
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
{
//printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned));
current_messages.time_stamps.local_position_ned = get_time_usec();
this_timestamps.local_position_ned = current_messages.time_stamps.local_position_ned;
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
//printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
mavlink_msg_global_position_int_decode(&message, &(current_messages.global_position_int));
current_messages.time_stamps.global_position_int = get_time_usec();
this_timestamps.global_position_int = current_messages.time_stamps.global_position_int;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
{
//printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
mavlink_msg_position_target_local_ned_decode(&message, &(current_messages.position_target_local_ned));
current_messages.time_stamps.position_target_local_ned = get_time_usec();
this_timestamps.position_target_local_ned = current_messages.time_stamps.position_target_local_ned;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
{
//printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
mavlink_msg_position_target_global_int_decode(&message, &(current_messages.position_target_global_int));
current_messages.time_stamps.position_target_global_int = get_time_usec();
this_timestamps.position_target_global_int = current_messages.time_stamps.position_target_global_int;
break;
}
case MAVLINK_MSG_ID_HIGHRES_IMU:
{
//printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
clock_gettime(CLOCK_REALTIME,&receive_time);
mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu));
current_messages.time_stamps.highres_imu = get_time_usec();
this_timestamps.highres_imu = current_messages.time_stamps.highres_imu;
highres_imu_count++;
//printf("IMU Sensors Timestamp %u\n",current_messages.time_stamps.highres_imu/1000);
if ( (current_messages.time_stamps.highres_imu - read_highres_imu_old) > 10000000 )
{
printf("HIGHRES_IMU frequency : %d Hz\n",highres_imu_count/10);
highres_imu_count = 0;
read_highres_imu_old = current_messages.time_stamps.highres_imu;
}
//fprintf(logfile_rd,"HIGHRES_IMU %d, %ld \n",receive_time.tv_sec, receive_time.tv_nsec);
break;
}
case MAVLINK_MSG_ID_ATTITUDE:
{
//printf("MAVLINK_MSG_ID_ATTITUDE\n");
mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
current_messages.time_stamps.attitude = get_time_usec();
this_timestamps.attitude = current_messages.time_stamps.attitude;
break;
}
case MAVLINK_MSG_ID_HIL_CONTROLS:
{
//printf("MAVLINK_MSG_ID_HIL_CONTROLS\n");
mavlink_msg_hil_controls_decode(&message, &(current_messages.hil_controls));
current_messages.time_stamps.hil_controls = get_time_usec();
this_timestamps.hil_controls = current_messages.time_stamps.hil_controls;
float data[4] = {current_messages.hil_controls.roll_ailerons,
current_messages.hil_controls.pitch_elevator,
current_messages.hil_controls.yaw_rudder,
current_messages.hil_controls.throttle};
int bsent = udp_mtl->send_bytes((char *)data,sizeof(float[4]));
hil_controls_count++;
//printf("IMU Sensors Timestamp %u\n",current_messages.time_stamps.highres_imu/1000);
if ( (current_messages.time_stamps.hil_controls - read_hil_controls_old) > 10000000 )
{
printf("HIL_CONTROLS frequency : %d Hz\n",hil_controls_count/10);
hil_controls_count = 0;
read_hil_controls_old = current_messages.time_stamps.hil_controls;
printf("%0.2f | ",current_messages.hil_controls.roll_ailerons);
printf("%0.2f | ",current_messages.hil_controls.pitch_elevator);
printf("%0.2f | ",current_messages.hil_controls.yaw_rudder);
printf("%0.2f | \n",current_messages.hil_controls.throttle);
}
//fprintf(logfile_rd,"HIL_CONTROLS %d, %ld \n",receive_time.tv_sec, receive_time.tv_nsec);
break;
}
default:
{
// printf("Warning, did not handle message id %i\n",message.msgid);
break;
}
} // end: switch msgid
return;
}
void _Mavlink::handleMessages()
{
mavlink_message_t message;
int nMsgHandled = 0;
//Handle Message while new message is received
while (readMessage(message))
{
// Note this doesn't handle multiple message sources.
m_msg.sysid = message.sysid;
m_msg.compid = message.compid;
// Handle Message ID
switch (message.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
LOG(INFO)<<"-> MAVLINK_MSG_ID_HEARTBEAT";
mavlink_msg_heartbeat_decode(&message, &(m_msg.heartbeat));
m_msg.time_stamps.heartbeat = get_time_usec();
if (m_msg.heartbeat.type != MAV_TYPE_GCS)
{
m_systemID = m_msg.sysid;
m_targetComponentID = m_msg.compid;
LOG_I("-> SYSTEM_ID:"<<m_systemID
<<" COMPONENT_ID:"<<m_componentID
<<" TARGET_COMPONENT_ID:"<<m_targetComponentID);
}
else
{
LOG_I("->HEARTBEAT FROM MAV_TYPE_GCS");
}
break;
}
case MAVLINK_MSG_ID_SYS_STATUS:
{
LOG_I("-> MAVLINK_MSG_ID_SYS_STATUS");
mavlink_msg_sys_status_decode(&message,
&(m_msg.sys_status));
m_msg.time_stamps.sys_status = get_time_usec();
break;
}
case MAVLINK_MSG_ID_BATTERY_STATUS:
{
LOG_I("-> MAVLINK_MSG_ID_BATTERY_STATUS");
mavlink_msg_battery_status_decode(&message,
&(m_msg.battery_status));
m_msg.time_stamps.battery_status = get_time_usec();
break;
}
case MAVLINK_MSG_ID_RADIO_STATUS:
{
LOG_I("-> MAVLINK_MSG_ID_RADIO_STATUS");
mavlink_msg_radio_status_decode(&message,
&(m_msg.radio_status));
m_msg.time_stamps.radio_status = get_time_usec();
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
{
LOG_I("-> MAVLINK_MSG_ID_LOCAL_POSITION_NED");
mavlink_msg_local_position_ned_decode(&message,
&(m_msg.local_position_ned));
m_msg.time_stamps.local_position_ned = get_time_usec();
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
LOG_I("-> MAVLINK_MSG_ID_GLOBAL_POSITION_INT");
mavlink_msg_global_position_int_decode(&message,
&(m_msg.global_position_int));
m_msg.time_stamps.global_position_int = get_time_usec();
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
{
LOG_I("-> MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED");
mavlink_msg_position_target_local_ned_decode(&message,
&(m_msg.position_target_local_ned));
m_msg.time_stamps.position_target_local_ned =
get_time_usec();
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
{
LOG_I("-> MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT");
mavlink_msg_position_target_global_int_decode(&message,
&(m_msg.position_target_global_int));
m_msg.time_stamps.position_target_global_int =
get_time_usec();
break;
}
case MAVLINK_MSG_ID_HIGHRES_IMU:
{
LOG_I("-> MAVLINK_MSG_ID_HIGHRES_IMU");
mavlink_msg_highres_imu_decode(&message,
&(m_msg.highres_imu));
m_msg.time_stamps.highres_imu = get_time_usec();
break;
}
case MAVLINK_MSG_ID_ATTITUDE:
{
LOG_I("-> MAVLINK_MSG_ID_ATTITUDE");
mavlink_msg_attitude_decode(&message, &(m_msg.attitude));
m_msg.time_stamps.attitude = get_time_usec();
break;
}
case MAVLINK_MSG_ID_COMMAND_ACK:
{
mavlink_msg_command_ack_decode(&message,
&(m_msg.command_ack));
m_msg.time_stamps.attitude = get_time_usec();
LOG_I("-> MAVLINK_MSG_ID_COMMAND_ACK:"<<m_msg.command_ack.result);
break;
}
default:
{
LOG_I("-> UNKNOWN MSG_ID:"<<message.msgid);
break;
}
}
if (++nMsgHandled >= NUM_MSG_HANDLE)
return;
}
}
void MavlinkComm::_handleMessage(mavlink_message_t * msg) {
uint32_t timeStamp = micros();
switch (msg->msgid) {
_board->debug->printf_P(PSTR("message received: %d"), msg->msgid);
case MAVLINK_MSG_ID_HEARTBEAT: {
mavlink_heartbeat_t packet;
mavlink_msg_heartbeat_decode(msg, &packet);
_lastHeartBeat = micros();
break;
}
case MAVLINK_MSG_ID_GPS_RAW: {
// decode
mavlink_gps_raw_t packet;
mavlink_msg_gps_raw_decode(msg, &packet);
_navigator->setTimeStamp(timeStamp);
_navigator->setLat(packet.lat * deg2Rad);
_navigator->setLon(packet.lon * deg2Rad);
_navigator->setAlt(packet.alt);
_navigator->setYaw(packet.hdg * deg2Rad);
_navigator->setGroundSpeed(packet.v);
_navigator->setAirSpeed(packet.v);
//_board->debug->printf_P(PSTR("received hil gps raw packet\n"));
/*
_board->debug->printf_P(PSTR("received lat: %f deg\tlon: %f deg\talt: %f m\n"),
packet.lat,
packet.lon,
packet.alt);
*/
break;
}
case MAVLINK_MSG_ID_HIL_STATE: {
// decode
mavlink_hil_state_t packet;
mavlink_msg_hil_state_decode(msg, &packet);
_navigator->setTimeStamp(timeStamp);
_navigator->setRoll(packet.roll);
_navigator->setPitch(packet.pitch);
_navigator->setYaw(packet.yaw);
_navigator->setRollRate(packet.rollspeed);
_navigator->setPitchRate(packet.pitchspeed);
_navigator->setYawRate(packet.yawspeed);
_navigator->setVN(packet.vx/ 1e2);
_navigator->setVE(packet.vy/ 1e2);
_navigator->setVD(packet.vz/ 1e2);
_navigator->setLat_degInt(packet.lat);
_navigator->setLon_degInt(packet.lon);
_navigator->setAlt(packet.alt / 1e3);
_navigator->setXAccel(packet.xacc/ 1e3);
_navigator->setYAccel(packet.xacc/ 1e3);
_navigator->setZAccel(packet.xacc/ 1e3);
break;
}
case MAVLINK_MSG_ID_ATTITUDE: {
// decode
mavlink_attitude_t packet;
mavlink_msg_attitude_decode(msg, &packet);
// set dcm hil sensor
_navigator->setTimeStamp(timeStamp);
_navigator->setRoll(packet.roll);
_navigator->setPitch(packet.pitch);
_navigator->setYaw(packet.yaw);
_navigator->setRollRate(packet.rollspeed);
_navigator->setPitchRate(packet.pitchspeed);
_navigator->setYawRate(packet.yawspeed);
//_board->debug->printf_P(PSTR("received hil attitude packet\n"));
break;
}
case MAVLINK_MSG_ID_ACTION: {
// decode
mavlink_action_t packet;
mavlink_msg_action_decode(msg, &packet);
if (_checkTarget(packet.target, packet.target_component))
break;
// do action
sendText(SEVERITY_LOW, PSTR("action received"));
switch (packet.action) {
case MAV_ACTION_STORAGE_READ:
AP_Var::load_all();
break;
case MAV_ACTION_STORAGE_WRITE:
AP_Var::save_all();
break;
case MAV_ACTION_MOTORS_START:
_controller->setMode(MAV_MODE_READY);
break;
case MAV_ACTION_CALIBRATE_GYRO:
case MAV_ACTION_CALIBRATE_MAG:
case MAV_ACTION_CALIBRATE_ACC:
case MAV_ACTION_CALIBRATE_PRESSURE:
_controller->setMode(MAV_MODE_LOCKED);
_navigator->calibrate();
break;
case MAV_ACTION_EMCY_KILL:
case MAV_ACTION_CONFIRM_KILL:
case MAV_ACTION_MOTORS_STOP:
case MAV_ACTION_SHUTDOWN:
_controller->setMode(MAV_MODE_LOCKED);
break;
case MAV_ACTION_LAUNCH:
case MAV_ACTION_TAKEOFF:
_guide->setMode(MAV_NAV_LIFTOFF);
break;
case MAV_ACTION_LAND:
_guide->setCurrentIndex(0);
_guide->setMode(MAV_NAV_LANDING);
break;
case MAV_ACTION_EMCY_LAND:
_guide->setMode(MAV_NAV_LANDING);
break;
case MAV_ACTION_LOITER:
case MAV_ACTION_HALT:
_guide->setMode(MAV_NAV_LOITER);
break;
case MAV_ACTION_SET_AUTO:
_controller->setMode(MAV_MODE_AUTO);
break;
case MAV_ACTION_SET_MANUAL:
_controller->setMode(MAV_MODE_MANUAL);
break;
case MAV_ACTION_RETURN:
_guide->setMode(MAV_NAV_RETURNING);
break;
case MAV_ACTION_NAVIGATE:
case MAV_ACTION_CONTINUE:
_guide->setMode(MAV_NAV_WAYPOINT);
break;
case MAV_ACTION_CALIBRATE_RC:
case MAV_ACTION_REBOOT:
case MAV_ACTION_REC_START:
case MAV_ACTION_REC_PAUSE:
case MAV_ACTION_REC_STOP:
sendText(SEVERITY_LOW, PSTR("action not implemented"));
break;
default:
sendText(SEVERITY_LOW, PSTR("unknown action"));
break;
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: {
sendText(SEVERITY_LOW, PSTR("waypoint request list"));
// decode
mavlink_waypoint_request_list_t packet;
mavlink_msg_waypoint_request_list_decode(msg, &packet);
if (_checkTarget(packet.target_system, packet.target_component))
break;
// Start sending waypoints
mavlink_msg_waypoint_count_send(_channel, msg->sysid, msg->compid,
_guide->getNumberOfCommands());
_cmdTimeLastSent = millis();
_cmdTimeLastReceived = millis();
_sendingCmds = true;
_receivingCmds = false;
_cmdDestSysId = msg->sysid;
_cmdDestCompId = msg->compid;
break;
}
case MAVLINK_MSG_ID_WAYPOINT_REQUEST: {
sendText(SEVERITY_LOW, PSTR("waypoint request"));
// Check if sending waypiont
if (!_sendingCmds)
break;
// decode
mavlink_waypoint_request_t packet;
mavlink_msg_waypoint_request_decode(msg, &packet);
if (_checkTarget(packet.target_system, packet.target_component))
break;
_board->debug->printf_P(PSTR("sequence: %d\n"),packet.seq);
AP_MavlinkCommand cmd(packet.seq);
mavlink_waypoint_t wp = cmd.convert(_guide->getCurrentIndex());
mavlink_msg_waypoint_send(_channel, _cmdDestSysId, _cmdDestCompId,
wp.seq, wp.frame, wp.command, wp.current, wp.autocontinue,
wp.param1, wp.param2, wp.param3, wp.param4, wp.x, wp.y,
wp.z);
// update last waypoint comm stamp
_cmdTimeLastSent = millis();
break;
}
case MAVLINK_MSG_ID_WAYPOINT_ACK: {
sendText(SEVERITY_LOW, PSTR("waypoint ack"));
// decode
mavlink_waypoint_ack_t packet;
mavlink_msg_waypoint_ack_decode(msg, &packet);
if (_checkTarget(packet.target_system, packet.target_component))
break;
// check for error
//uint8_t type = packet.type; // ok (0), error(1)
// turn off waypoint send
_sendingCmds = false;
break;
}
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
sendText(SEVERITY_LOW, PSTR("param request list"));
// decode
mavlink_param_request_list_t packet;
mavlink_msg_param_request_list_decode(msg, &packet);
if (_checkTarget(packet.target_system, packet.target_component))
break;
// Start sending parameters - next call to ::update will kick the first one out
_queuedParameter = AP_Var::first();
_queuedParameterIndex = 0;
break;
}
case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: {
sendText(SEVERITY_LOW, PSTR("waypoint clear all"));
// decode
mavlink_waypoint_clear_all_t packet;
mavlink_msg_waypoint_clear_all_decode(msg, &packet);
if (_checkTarget(packet.target_system, packet.target_component))
break;
// clear all waypoints
uint8_t type = 0; // ok (0), error(1)
_guide->setNumberOfCommands(1);
_guide->setCurrentIndex(0);
// send acknowledgement 3 times to makes sure it is received
for (int i = 0; i < 3; i++)
mavlink_msg_waypoint_ack_send(_channel, msg->sysid,
msg->compid, type);
break;
}
case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: {
sendText(SEVERITY_LOW, PSTR("waypoint set current"));
// decode
mavlink_waypoint_set_current_t packet;
mavlink_msg_waypoint_set_current_decode(msg, &packet);
Serial.print("Packet Sequence:");
Serial.println(packet.seq);
if (_checkTarget(packet.target_system, packet.target_component))
break;
// set current waypoint
Serial.print("Current Index:");
Serial.println(_guide->getCurrentIndex());
Serial.flush();
_guide->setCurrentIndex(packet.seq);
mavlink_msg_waypoint_current_send(_channel,
_guide->getCurrentIndex());
break;
}
case MAVLINK_MSG_ID_WAYPOINT_COUNT: {
sendText(SEVERITY_LOW, PSTR("waypoint count"));
// decode
mavlink_waypoint_count_t packet;
mavlink_msg_waypoint_count_decode(msg, &packet);
if (_checkTarget(packet.target_system, packet.target_component))
break;
// start waypoint receiving
if (packet.count > _cmdMax) {
packet.count = _cmdMax;
}
_cmdNumberRequested = packet.count;
_cmdTimeLastReceived = millis();
_receivingCmds = true;
_sendingCmds = false;
_cmdRequestIndex = 0;
break;
}
case MAVLINK_MSG_ID_WAYPOINT: {
sendText(SEVERITY_LOW, PSTR("waypoint"));
// Check if receiving waypiont
if (!_receivingCmds) {
//sendText(SEVERITY_HIGH, PSTR("not receiving commands"));
break;
}
// decode
mavlink_waypoint_t packet;
mavlink_msg_waypoint_decode(msg, &packet);
if (_checkTarget(packet.target_system, packet.target_component))
break;
// check if this is the requested waypoint
if (packet.seq != _cmdRequestIndex) {
char warningMsg[50];
sprintf(warningMsg,
"waypoint request out of sequence: (packet) %d / %d (ap)",
packet.seq, _cmdRequestIndex);
sendText(SEVERITY_HIGH, warningMsg);
break;
}
_board->debug->printf_P(PSTR("received waypoint x: %f\ty: %f\tz: %f\n"),
packet.x,
packet.y,
packet.z);
// store waypoint
AP_MavlinkCommand command(packet);
//sendText(SEVERITY_HIGH, PSTR("waypoint stored"));
_cmdRequestIndex++;
if (_cmdRequestIndex == _cmdNumberRequested) {
sendMessage(MAVLINK_MSG_ID_WAYPOINT_ACK);
_receivingCmds = false;
_guide->setNumberOfCommands(_cmdNumberRequested);
// make sure curernt waypoint still exists
if (_cmdNumberRequested > _guide->getCurrentIndex()) {
_guide->setCurrentIndex(0);
mavlink_msg_waypoint_current_send(_channel,
_guide->getCurrentIndex());
}
//sendText(SEVERITY_LOW, PSTR("waypoint ack sent"));
} else if (_cmdRequestIndex > _cmdNumberRequested) {
_receivingCmds = false;
}
_cmdTimeLastReceived = millis();
break;
}
case MAVLINK_MSG_ID_PARAM_SET: {
sendText(SEVERITY_LOW, PSTR("param set"));
AP_Var *vp;
AP_Meta_class::Type_id var_type;
// decode
mavlink_param_set_t packet;
mavlink_msg_param_set_decode(msg, &packet);
if (_checkTarget(packet.target_system, packet.target_component))
break;
// set parameter
char key[_paramNameLengthMax + 1];
strncpy(key, (char *) packet.param_id, _paramNameLengthMax);
key[_paramNameLengthMax] = 0;
// find the requested parameter
vp = AP_Var::find(key);
if ((NULL != vp) && // exists
!isnan(packet.param_value) && // not nan
!isinf(packet.param_value)) { // not inf
// add a small amount before casting parameter values
// from float to integer to avoid truncating to the
// next lower integer value.
const float rounding_addition = 0.01;
// fetch the variable type ID
var_type = vp->meta_type_id();
// handle variables with standard type IDs
if (var_type == AP_Var::k_typeid_float) {
((AP_Float *) vp)->set_and_save(packet.param_value);
} else if (var_type == AP_Var::k_typeid_float16) {
((AP_Float16 *) vp)->set_and_save(packet.param_value);
} else if (var_type == AP_Var::k_typeid_int32) {
((AP_Int32 *) vp)->set_and_save(
packet.param_value + rounding_addition);
} else if (var_type == AP_Var::k_typeid_int16) {
((AP_Int16 *) vp)->set_and_save(
packet.param_value + rounding_addition);
} else if (var_type == AP_Var::k_typeid_int8) {
((AP_Int8 *) vp)->set_and_save(
packet.param_value + rounding_addition);
} else {
// we don't support mavlink set on this parameter
break;
}
// Report back the new value if we accepted the change
// we send the value we actually set, which could be
// different from the value sent, in case someone sent
// a fractional value to an integer type
mavlink_msg_param_value_send(_channel, (int8_t *) key,
vp->cast_to_float(), _countParameters(), -1); // XXX we don't actually know what its index is...
}
break;
} // end case
}
}
// ------------------------------------------------------------------------------
// Read Messages
// ------------------------------------------------------------------------------
void Autopilot_Interface::read_messages() {
bool success; // receive success flag
bool received_all = false; // receive only one message
Time_Stamps this_timestamps;
// Blocking wait for new data
while (!received_all and !time_to_exit) {
// ----------------------------------------------------------------------
// READ MESSAGE
// ----------------------------------------------------------------------
mavlink_message_t message;
success = serial_port->read_message(message);
// ----------------------------------------------------------------------
// HANDLE MESSAGE
// ----------------------------------------------------------------------
if (success) {
// Store message sysid and compid.
// Note this doesn't handle multiple message sources.
current_messages.sysid = message.sysid;
current_messages.compid = message.compid;
// Handle Message ID
switch (message.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT: {
printf("MAVLINK_MSG_ID_HEARTBEAT\n");
mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));
current_messages.time_stamps.heartbeat = get_time_usec();
this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
break;
}
case MAVLINK_MSG_ID_SYS_STATUS: {
printf("MAVLINK_MSG_ID_SYS_STATUS\n");
mavlink_msg_sys_status_decode(&message,
&(current_messages.sys_status));
current_messages.time_stamps.sys_status = get_time_usec();
this_timestamps.sys_status = current_messages.time_stamps.sys_status;
printf("Battery life remaining: %i\n",
current_messages.sys_status.battery_remaining);
break;
}
// this is not being received
case MAVLINK_MSG_ID_BATTERY_STATUS: {
printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
mavlink_msg_battery_status_decode(&message,
&(current_messages.battery_status));
current_messages.time_stamps.battery_status = get_time_usec();
this_timestamps.battery_status =
current_messages.time_stamps.battery_status;
printf("Battery life remaining: %i\n",
current_messages.battery_status.battery_remaining);
break;
}
case MAVLINK_MSG_ID_RADIO_STATUS: {
printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
mavlink_msg_radio_status_decode(&message,
&(current_messages.radio_status));
current_messages.time_stamps.radio_status = get_time_usec();
this_timestamps.radio_status =
current_messages.time_stamps.radio_status;
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION_NED: {
printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
mavlink_msg_local_position_ned_decode(
&message, &(current_messages.local_position_ned));
printf(" pos (GPS): %f %f %f (m)\n",
current_messages.local_position_ned.x,
current_messages.local_position_ned.y,
current_messages.local_position_ned.z);
current_messages.time_stamps.local_position_ned = get_time_usec();
this_timestamps.local_position_ned =
current_messages.time_stamps.local_position_ned;
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
mavlink_msg_global_position_int_decode(
&message, &(current_messages.global_position_int));
current_messages.time_stamps.global_position_int = get_time_usec();
this_timestamps.global_position_int =
current_messages.time_stamps.global_position_int;
mavlink_global_position_int_t gpos =
current_messages.global_position_int;
printf(" pos (GPS): %f %f %f (m)\n", gpos.lat, gpos.lon,
gpos.alt);
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED: {
printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
mavlink_msg_position_target_local_ned_decode(
&message, &(current_messages.position_target_local_ned));
current_messages.time_stamps.position_target_local_ned =
get_time_usec();
this_timestamps.position_target_local_ned =
current_messages.time_stamps.position_target_local_ned;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT: {
printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
mavlink_msg_position_target_global_int_decode(
&message, &(current_messages.position_target_global_int));
current_messages.time_stamps.position_target_global_int =
get_time_usec();
this_timestamps.position_target_global_int =
current_messages.time_stamps.position_target_global_int;
break;
}
case MAVLINK_MSG_ID_HIGHRES_IMU: {
printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
mavlink_msg_highres_imu_decode(&message,
&(current_messages.highres_imu));
current_messages.time_stamps.highres_imu = get_time_usec();
this_timestamps.highres_imu =
current_messages.time_stamps.highres_imu;
mavlink_highres_imu_t imu = current_messages.highres_imu;
printf(" ap time: %llu \n", imu.time_usec);
printf(" acc (NED): % f % f % f (m/s^2)\n", imu.xacc, imu.yacc,
imu.zacc);
printf(" gyro (NED): % f % f % f (rad/s)\n", imu.xgyro, imu.ygyro,
imu.zgyro);
printf(" mag (NED): % f % f % f (Ga)\n", imu.xmag, imu.ymag,
imu.zmag);
printf(" baro: %f (mBar) \n", imu.abs_pressure);
printf(" altitude: %f (m) \n", imu.pressure_alt);
printf(" temperature: %f C \n", imu.temperature);
break;
}
case MAVLINK_MSG_ID_ATTITUDE: {
printf("### MAVLINK_MSG_ID_ATTITUDE\n");
mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
current_messages.time_stamps.attitude = get_time_usec();
this_timestamps.attitude = current_messages.time_stamps.attitude;
mavlink_attitude_t att = current_messages.attitude;
printf("### roll: %f pitch: %f yaw: %f\n", att.roll, att.pitch,
att.yaw);
break;
}
default: {
// printf("Warning, did not handle message id %i\n",message.msgid);
break;
}
} // end: switch msgid
} // end: if read message
// Check for receipt of all items
received_all =
this_timestamps.heartbeat &&
// this_timestamps.battery_status &&
// this_timestamps.radio_status &&
// this_timestamps.local_position_ned &&
// this_timestamps.global_position_int &&
// this_timestamps.position_target_local_ned
//&&
// this_timestamps.position_target_global_int
//&&
// this_timestamps.highres_imu &&
// this_timestamps.attitude &&
this_timestamps.sys_status;
// give the write thread time to use the port
if (writing_status > false) {
usleep(100); // look for components of batches at 10kHz
}
} // end: while not received all
return;
}
// ------------------------------------------------------------------------------
// Read Messages
// ------------------------------------------------------------------------------
void
Autopilot_Interface::
read_messages(FILE *fd)
{
bool success; // receive success flag
bool received_all = false; // receive only one message
Time_Stamps this_timestamps;
// fprintf(fd," xacc yacc zacc xgyro ygyro zgyro xmag ymag zmag altitude temperature\n");
// Blocking wait for new data
while ( !received_all and !time_to_exit )
{
// ----------------------------------------------------------------------
// READ MESSAGE
// ----------------------------------------------------------------------
mavlink_message_t message;
success = serial_port->read_message(message);
// printf("messageid = %d\n", message.msgid);
printf("flag = %d ch = %d \n",receive_flag,receive_ch);
// ----------------------------------------------------------------------
// HANDLE MESSAGE
// ----------------------------------------------------------------------
if( success )
{
// Store message sysid and compid.
// Note this doesn't handle multiple message sources.
current_messages.sysid = message.sysid;
current_messages.compid = message.compid;
// Handle Message ID
switch (message.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
//printf("MAVLINK_MSG_ID_HEARTBEAT\n");
mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));
current_messages.time_stamps.heartbeat = get_time_usec();
this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
break;
}
case MAVLINK_MSG_ID_SYS_STATUS:
{
//printf("MAVLINK_MSG_ID_SYS_STATUS\n");
mavlink_msg_sys_status_decode(&message, &(current_messages.sys_status));
current_messages.time_stamps.sys_status = get_time_usec();
this_timestamps.sys_status = current_messages.time_stamps.sys_status;
break;
}
case MAVLINK_MSG_ID_BATTERY_STATUS:
{
//printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
mavlink_msg_battery_status_decode(&message, &(current_messages.battery_status));
current_messages.time_stamps.battery_status = get_time_usec();
this_timestamps.battery_status = current_messages.time_stamps.battery_status;
break;
}
case MAVLINK_MSG_ID_RADIO_STATUS:
{
//printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
mavlink_msg_radio_status_decode(&message, &(current_messages.radio_status));
current_messages.time_stamps.radio_status = get_time_usec();
this_timestamps.radio_status = current_messages.time_stamps.radio_status;
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
{
//printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned));
//fprintf(fd,"x = %f y = %f z = %f \n",current_messages.local_position_ned.x,current_messages.local_position_ned.y,current_messages.local_position_ned.z);
current_messages.time_stamps.local_position_ned = get_time_usec();
this_timestamps.local_position_ned = current_messages.time_stamps.local_position_ned;
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
//printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
mavlink_msg_global_position_int_decode(&message, &(current_messages.global_position_int));
current_messages.time_stamps.global_position_int = get_time_usec();
this_timestamps.global_position_int = current_messages.time_stamps.global_position_int;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
{
//printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
mavlink_msg_position_target_local_ned_decode(&message, &(current_messages.position_target_local_ned));
current_messages.time_stamps.position_target_local_ned = get_time_usec();
this_timestamps.position_target_local_ned = current_messages.time_stamps.position_target_local_ned;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
{
//printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
mavlink_msg_position_target_global_int_decode(&message, &(current_messages.position_target_global_int));
fprintf(fd, "lat_int = %d\n", current_messages.position_target_global_int.lat_int);
current_messages.time_stamps.position_target_global_int = get_time_usec();
this_timestamps.position_target_global_int = current_messages.time_stamps.position_target_global_int;
break;
}
case MAVLINK_MSG_ID_HIGHRES_IMU:
{
//printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu));
// fprintf(fd,"acc: %f %f %f ",current_messages.highres_imu.xacc,current_messages.highres_imu.yacc,current_messages.highres_imu.zacc);
// fprintf(fd,"gyro: %f %f %f ",current_messages.highres_imu.xgyro,current_messages.highres_imu.ygyro,current_messages.highres_imu.zgyro);
// fprintf(fd,"mag: %f %f %f ",current_messages.highres_imu.xmag,current_messages.highres_imu.ymag,current_messages.highres_imu.zmag);
// fprintf(fd,"alt: %f %f\n",current_messages.highres_imu.pressure_alt,current_messages.highres_imu.temperature);
if (receive_flag && receive_ch == 10) {
fprintf(fd,"acc: %f %f %f ",current_messages.highres_imu.xacc,current_messages.highres_imu.yacc,current_messages.highres_imu.zacc);
fprintf(fd,"gyro: %f %f %f ",current_messages.highres_imu.xgyro,current_messages.highres_imu.ygyro,current_messages.highres_imu.zgyro);
fprintf(fd,"mag: %f %f %f ",current_messages.highres_imu.xmag,current_messages.highres_imu.ymag,current_messages.highres_imu.zmag);
fprintf(fd,"alt: %f %f\n",current_messages.highres_imu.pressure_alt,current_messages.highres_imu.temperature);
receive_flag = false;
}
current_messages.time_stamps.highres_imu = get_time_usec();
this_timestamps.highres_imu = current_messages.time_stamps.highres_imu;
break;
}
case MAVLINK_MSG_ID_GPS_RAW_INT:
{
mavlink_msg_gps_raw_int_decode(&message, &(current_messages.gps_raw_int));
fprintf(fd, "lat = %d lon = %d alt = %d ", current_messages.gps_raw_int.lat,current_messages.gps_raw_int.lon,current_messages.gps_raw_int.alt);
fprintf(fd, "vel = %d cog = %d satellites_visible = %d\n", current_messages.gps_raw_int.vel,current_messages.gps_raw_int.cog,current_messages.gps_raw_int.satellites_visible);
if (receive_flag && receive_ch == 10) {
fprintf(fd, "lat = %d lon = %d alt = %d ", current_messages.gps_raw_int.lat,current_messages.gps_raw_int.lon,current_messages.gps_raw_int.alt);
fprintf(fd, "vel = %d cog = %d satellites_visible = %d\n", current_messages.gps_raw_int.vel,current_messages.gps_raw_int.cog,current_messages.gps_raw_int.satellites_visible);
receive_flag = false;
}
break;
}
case MAVLINK_MSG_ID_ATTITUDE:
{
//printf("MAVLINK_MSG_ID_ATTITUDE\n");
mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
current_messages.time_stamps.attitude = get_time_usec();
this_timestamps.attitude = current_messages.time_stamps.attitude;
break;
}
default:
{
// printf("Warning, did not handle message id %i\n",message.msgid);
break;
}
} // end: switch msgid
} // end: if read message
// Check for receipt of all items
received_all =
this_timestamps.heartbeat &&
// this_timestamps.battery_status &&
// this_timestamps.radio_status &&
// this_timestamps.local_position_ned &&
// this_timestamps.global_position_int &&
// this_timestamps.position_target_local_ned &&
// this_timestamps.position_target_global_int &&
// this_timestamps.highres_imu &&
// this_timestamps.attitude &&
this_timestamps.sys_status
;
// give the write thread time to use the port
if ( writing_status > false ) {
usleep(100); // look for components of batches at 10kHz
}
} // end: while not received all
return;
}
void ofxMavlink::threadedFunction() {
while( isThreadRunning() != 0 ) {
// static unsigned int imu_receive_counter =
uint8_t cp;
mavlink_message_t message;
mavlink_status_t status;
uint8_t msgReceived = false;
if (read(fd, &cp, 1) > 0)
{
// Check if a message could be decoded, return the message in case yes
msgReceived = mavlink_parse_char(MAVLINK_COMM_1, cp, &message, &status);
if (status.packet_rx_drop_count > 0)
{
if(bDebug) ofLogWarning("ofxMavlink") << "ERROR: DROPPED " << status.packet_rx_drop_count <<" PACKETS:" << " " << cp;
}
}
else
{
if(bDebug) ofLogError("ofxMavlink") << "ERROR: Could not read from fd " << fd;
}
// If a message could be decoded, handle it
if(msgReceived)
{
msgcount++;
if (bDebug)
{
fprintf(stderr,"Received serial data: ");
unsigned int i;
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
unsigned int messageLength = mavlink_msg_to_send_buffer(buffer, &message);
if (messageLength > MAVLINK_MAX_PACKET_LEN)
{
fprintf(stderr, "\nFATAL ERROR: MESSAGE LENGTH IS LARGER THAN BUFFER SIZE\n");
}
else
{
for (i=0; i<messageLength; i++)
{
unsigned char v=buffer[i];
fprintf(stderr,"%02x ", v);
}
fprintf(stderr,"\n");
}
}
//if(bDebug) ofLogNotice("ofxMavlink") << "Received message from serial with ID #" << message.msgid << " (sys:" << message.sysid << "|comp:" << message.compid << "):\n";
switch (message.msgid)
{
case MAVLINK_MSG_ID_COMMAND_ACK:
{
mavlink_command_ack_t ack;
mavlink_msg_command_ack_decode(&message, &ack);
switch (ack.result)
{
case MAV_RESULT_ACCEPTED:
{
ofLogNotice("ofxMavlink") << "SUCCESS: Executed Command: " << ack.command;
}
break;
case MAV_RESULT_TEMPORARILY_REJECTED:
{
ofLogError("ofxMavlink") << "FAILURE: Temporarily rejected Command: " << ack.command;
}
break;
case MAV_RESULT_DENIED:
{
ofLogError("ofxMavlink") << "FAILURE: Denied Command: " << ack.command;
}
break;
case MAV_RESULT_UNSUPPORTED:
{
ofLogError("ofxMavlink") << "FAILURE: Unsupported Command: " << ack.command;
}
break;
case MAV_RESULT_FAILED:
{
ofLogError("ofxMavlink") << "FAILURE: Failed Command: " << ack.command;
}
break;
}
break;
}
// case MAVLINK_MSG_ID_COMMAND_ACK:
// {
// mavlink_command_ack_t ack;
// mavlink_msg_command_ack_decode(&message,&ack);
// cout << "received CMD_ACK command=" << ack.command << " result=" << ack.result << endl;
//
// if(ack.command == MAV_CMD_COMPONENT_ARM_DISARM) {
// cout << "Acknowledge arm/disarm command";
//
// }
// if(ack.result == MAV_CMD_ACK_OK) {
// cout << "received CMD_ACK_OK" << endl;
// }
// break;
// }
case MAVLINK_MSG_ID_RAW_IMU:
{
mavlink_raw_imu_t imu;
mavlink_msg_raw_imu_decode(&message, &imu);
lock();
time_usec = imu.time_usec;
unlock();
ofLogVerbose("ofxMavlink") << "-- RAW_IMU message received --" << endl;
ofLogVerbose("ofxMavlink") << "\t time: (us) " << imu.time_usec << endl;
ofLogVerbose("ofxMavlink") << "\t acc:" << imu.xacc << " " << imu.yacc << " " << imu.zacc << endl;
ofLogVerbose("ofxMavlink") << "\t gyro: (rad/s)" << imu.xgyro << " " << imu.ygyro << " " << imu.zgyro << endl;
ofLogVerbose("ofxMavlink") << "\t mag: (Ga)" << imu.xmag << " " << imu.ymag << " " << imu.zmag << endl;
break;
}
case MAVLINK_MSG_ID_GPS_RAW_INT:
{
ofLogNotice("ofxMavlink") << "-- GPS RAW INT message received --" << endl;
mavlink_gps_raw_int_t packet;
mavlink_msg_gps_raw_int_decode(&message, &packet);
if (packet.fix_type > 2)
{
lock();
latitude = packet.lat/(double)1E7;
longitude = packet.lon/(double)1E7;
altitude = packet.alt/1000.0;
unlock();
ofLogNotice("ofxMavlink") << "Altitude:" << packet.alt/1000.0 << "latitude:" << (float)packet.lat / 10000000.0 << " longitude:" << (float)packet.lon / 10000000.0 << " ";
ofLogNotice("ofxMavlink") << "Satellites visible:" << packet.satellites_visible << " GPS fix:" << packet.fix_type << endl;
// if (packet.lat != 0.0) {
//
// latitude = (float)packet.lat;
// longitude = (float)packet.lon;
// altitude = (float)packet.alt;
// //ModelData.speed = (float)packet.vel / 100.0;
// //ModelData.numSat = packet.satellites_visible;
// gpsfix = packet.fix_type;
}
break;
}
case MAVLINK_MSG_ID_HEARTBEAT:
{
mavlink_heartbeat_t packet;
mavlink_msg_heartbeat_decode(&message, &packet);
int droneType = packet.type;
int autoPilot = packet.autopilot;
// cout << "base mode:" << packet.base_mode << " custom mode:" << packet.custom_mode << endl;
// if (packet.base_mode == MAV_MODE_MANUAL_ARMED) {
// //ModelData.mode = MODEL_MODE_MANUAL;
// cout << "Manual Mode" << endl;
// } else if (packet.base_mode == 128 + 64 + 16) {
// //ModelData.mode = MODEL_MODE_RTL;
// cout << "RTL Mode" << endl;
// } else if (packet.base_mode == 128 + 16) {
// //ModelData.mode = MODEL_MODE_POSHOLD;
// cout << "Poshold Mode" << endl;
// } else if (packet.base_mode == 128 + 4) {
// //ModelData.mode = MODEL_MODE_MISSION;
// cout << "Mission Mode" << endl;
// }
// if(packet.base_mode == MAV_MODE_STABILIZE_DISARMED)
// {
// cout << "Stablilize disarmed mode" << endl;
// }
ofLogNotice("ofxMavlink") << "-- Heartbeat -- sysId:" << message.sysid << " compId:" << message.compid << " drone type:" << droneType << " autoPilot:" << autoPilot;
// if(droneType == MAV_TYPE_QUADROTOR) cout << " Quadrotor"; else cout << droneType;
// cout << " Autopilot:";
// if(autoPilot == MAV_AUTOPILOT_ARDUPILOTMEGA) cout << " ArduPilotMega"; else cout << autoPilot;
// cout << endl;
if (message.sysid != 0xff) {
lock();
targetId = message.sysid;
compId = message.compid;
//cout << "compid = " << message.compid << " sysid =" << message.sysid;
unlock();
}
break;
}
default:
break;
}
}
}
}
// ------------------------------------------------------------------------------
// Read Messages
// ------------------------------------------------------------------------------
void
Autopilot_Interface::
read_messages()
{
bool success; // receive success flag
bool received_all = false; // receive only one message
Time_Stamps this_timestamps;
// Blocking wait for new data
while ( not received_all and not time_to_exit )
{
// ----------------------------------------------------------------------
// READ MESSAGE
// ----------------------------------------------------------------------
mavlink_message_t message;
success = serial_port->read_message(message);
// ----------------------------------------------------------------------
// HANDLE MESSAGE
// ----------------------------------------------------------------------
if( success )
{
// Store message sysid and compid.
// Note this doesn't handle multiple message sources.
current_messages.sysid = message.sysid;
current_messages.compid = message.compid;
// Handle Message ID
switch (message.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
std::cout << "MAVLINK_MSG_ID_HEARTBEAT" << std::endl;
mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));
current_messages.time_stamps.heartbeat = get_time_usec();
this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
break;
}
case MAVLINK_MSG_ID_SYS_STATUS:
{
//std::cout << "MAVLINK_MSG_ID_SYS_STATUS" << std::endl;
mavlink_msg_sys_status_decode(&message, &(current_messages.sys_status));
current_messages.time_stamps.sys_status = get_time_usec();
this_timestamps.sys_status = current_messages.time_stamps.sys_status;
break;
}
case MAVLINK_MSG_ID_BATTERY_STATUS:
{
std::cout << "MAVLINK_MSG_ID_BATTERY_STATUS" << std::endl;
mavlink_msg_battery_status_decode(&message, &(current_messages.battery_status));
current_messages.time_stamps.battery_status = get_time_usec();
this_timestamps.battery_status = current_messages.time_stamps.battery_status;
break;
}
case MAVLINK_MSG_ID_RADIO_STATUS:
{
std::cout << "MAVLINK_MSG_ID_RADIO_STATUS" << std::endl;
mavlink_msg_radio_status_decode(&message, &(current_messages.radio_status));
current_messages.time_stamps.radio_status = get_time_usec();
this_timestamps.radio_status = current_messages.time_stamps.radio_status;
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
{
std::cout << "MAVLINK_MSG_ID_LOCAL_POSITION_NED" << std::endl;
mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned));
current_messages.time_stamps.local_position_ned = get_time_usec();
this_timestamps.local_position_ned = current_messages.time_stamps.local_position_ned;
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
std::cout << "MAVLINK_MSG_ID_GLOBAL_POSITION_INT" << std::endl;
mavlink_msg_global_position_int_decode(&message, &(current_messages.global_position_int));
current_messages.time_stamps.global_position_int = get_time_usec();
this_timestamps.global_position_int = current_messages.time_stamps.global_position_int;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
{
std::cout << "MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED" << std::endl;
mavlink_msg_position_target_local_ned_decode(&message, &(current_messages.position_target_local_ned));
current_messages.time_stamps.position_target_local_ned = get_time_usec();
this_timestamps.position_target_local_ned = current_messages.time_stamps.position_target_local_ned;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
{
std::cout << "MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT" << std::endl;
mavlink_msg_position_target_global_int_decode(&message, &(current_messages.position_target_global_int));
current_messages.time_stamps.position_target_global_int = get_time_usec();
this_timestamps.position_target_global_int = current_messages.time_stamps.position_target_global_int;
break;
}
case MAVLINK_MSG_ID_HIGHRES_IMU:
{
std::cout << "MAVLINK_MSG_ID_HIGHRES_IMU" << std::endl;
mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu));
current_messages.time_stamps.highres_imu = get_time_usec();
this_timestamps.highres_imu = current_messages.time_stamps.highres_imu;
break;
}
case MAVLINK_MSG_ID_ATTITUDE:
{
vector4d quaternion;
std::cout << "MAVLINK_MSG_ID_ATTITUDE" << std::endl;
mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
current_messages.time_stamps.attitude = get_time_usec();
this_timestamps.attitude = current_messages.time_stamps.attitude;
quaternion_from_euler(&quaternion, current_messages.attitude.roll,
current_messages.attitude.pitch, current_messages.attitude.yaw);
std::cout <<
"\troll: " << current_messages.attitude.roll <<
"\tpitch: " << current_messages.attitude.pitch <<
"\tyaw: " << current_messages.attitude.yaw <<
std::endl;
std::cout <<
"\trollspeed: " << current_messages.attitude.rollspeed <<
"\tpitchspeed: " << current_messages.attitude.pitchspeed <<
"\tyawspeed: " << current_messages.attitude.yawspeed <<
std::endl;
std::cout <<
"\tqx: " << quaternion.x <<
"\tqy: " << quaternion.y <<
"\tqz: " << quaternion.z <<
"\tqw: " << quaternion.w <<
std::endl;
break;
}
case MAVLINK_MSG_ID_DEBUG:
{
std::cout << "MAVLINK_MSG_ID_DEBUG" << std::endl;
/* mavlink_msg_debug_decode(&message, &(current_messages.attitude));
current_messages.time_stamps.attitude = get_time_usec();
this_timestamps.attitude = current_messages.time_stamps.attitude;*/
break;
}
case MAVLINK_MSG_ID_STATUSTEXT:
{
std::cout << "MAVLINK_MSG_ID_STATUSTEXT: ";
mavlink_msg_statustext_decode(&message, &(current_messages.statustext));
current_messages.statustext.text[50] = 0;
printf("%d - '%s'\n", current_messages.statustext.severity, current_messages.statustext.text);
current_messages.time_stamps.statustext = get_time_usec();
this_timestamps.statustext = current_messages.time_stamps.statustext;
break;
}
case MAVLINK_MSG_ID_RAW_IMU:
{
std::cout << "MAVLINK_MSG_ID_RAW_IMU:" << std::endl;
mavlink_msg_raw_imu_decode(&message, &(current_messages.raw_imu));
std::cout << "\tacc :\t" << current_messages.raw_imu.xacc <<
"\t" << current_messages.raw_imu.yacc <<
"\t" << current_messages.raw_imu.zacc <<
std::endl;
std::cout << "\tgyro:\t" << current_messages.raw_imu.xgyro <<
"\t" << current_messages.raw_imu.ygyro <<
"\t" << current_messages.raw_imu.zgyro <<
std::endl;
std::cout << "\tmag:\t" << current_messages.raw_imu.xmag <<
"\t" << current_messages.raw_imu.ymag <<
"\t" << current_messages.raw_imu.zmag <<
std::endl;
current_messages.time_stamps.raw_imu = get_time_usec();
this_timestamps.raw_imu = current_messages.time_stamps.raw_imu;
break;
}
case MAVLINK_MSG_ID_GPS_RAW_INT:
{
std::cout << "MAVLINK_MSG_ID_GPS_RAW_INT:" << std::endl;
mavlink_msg_gps_raw_int_decode(&message, &(current_messages.gps_raw_int));
current_messages.time_stamps.gps_raw_int = get_time_usec();
this_timestamps.gps_raw_int = current_messages.time_stamps.gps_raw_int;
std::cout <<
"\tlat: " << current_messages.gps_raw_int.lat <<
"\tlon: " << current_messages.gps_raw_int.lon <<
"\talt: " << current_messages.gps_raw_int.alt <<
std::endl;
std::cout <<
"\teph: " << current_messages.gps_raw_int.eph <<
"\tepv: " << current_messages.gps_raw_int.epv <<
std::endl;
std::cout <<
"\tvel: " << current_messages.gps_raw_int.vel <<
"\tcog: " << current_messages.gps_raw_int.cog <<
std::endl;
std::cout <<
"\tfix: " << (int)current_messages.gps_raw_int.fix_type <<
"\tsat: " << (int)current_messages.gps_raw_int.satellites_visible <<
std::endl;
break;
}
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
{
vector4d quaternion;
std::cout << "MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:" << std::endl;
mavlink_msg_nav_controller_output_decode(&message, &(current_messages.nav_controller_output));
current_messages.time_stamps.nav_controller_output = get_time_usec();
this_timestamps.nav_controller_output = current_messages.time_stamps.nav_controller_output;
quaternion_from_euler(&quaternion, current_messages.nav_controller_output.nav_roll,
current_messages.nav_controller_output.nav_pitch, current_messages.nav_controller_output.nav_bearing);
std::cout <<
"\tnav_roll: " << current_messages.nav_controller_output.nav_roll <<
"\tnav_pitch: " << current_messages.nav_controller_output.nav_pitch <<
"\tnav_bearing: " << current_messages.nav_controller_output.nav_bearing <<
std::endl;
std::cout <<
"\tqx: " << quaternion.x <<
"\tqy: " << quaternion.y <<
"\tqz: " << quaternion.z <<
"\tqw: " << quaternion.w <<
std::endl;
std::cout <<
"\ttarget_bearing: " << current_messages.nav_controller_output.target_bearing <<
"\twp_dist: " << current_messages.nav_controller_output.wp_dist <<
std::endl;
std::cout <<
"\talt_error: " << current_messages.nav_controller_output.alt_error <<
"\taspd_error: " << current_messages.nav_controller_output.aspd_error <<
"\txtrack_error: " << current_messages.nav_controller_output.xtrack_error <<
std::endl;
break;
}
case MAVLINK_MSG_ID_SCALED_PRESSURE:
{
std::cout << "MAVLINK_MSG_ID_SCALED_PRESSURE:" << std::endl;
mavlink_msg_scaled_pressure_decode(&message, &(current_messages.scaled_pressure));
current_messages.time_stamps.scaled_pressure = get_time_usec();
this_timestamps.scaled_pressure = current_messages.time_stamps.scaled_pressure;
std::cout <<
"\tpress_abs: " << current_messages.scaled_pressure.press_abs <<
"\tpress_diff: " << current_messages.scaled_pressure.press_diff <<
"\ttemperature: " << ((double)current_messages.scaled_pressure.temperature / 100.0) <<
std::endl;
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
{
std::cout << "MAVLINK_MSG_ID_RC_CHANNELS_RAW:" << std::endl;
mavlink_msg_rc_channels_raw_decode(&message, &(current_messages.rc_channels_raw));
current_messages.time_stamps.rc_channels_raw = get_time_usec();
this_timestamps.rc_channels_raw = current_messages.time_stamps.rc_channels_raw;
std::cout <<
"\tport: " << (int)current_messages.rc_channels_raw.port <<
"\tchan1_raw: " << current_messages.rc_channels_raw.chan1_raw <<
"\tchan2_raw: " << current_messages.rc_channels_raw.chan2_raw <<
"\tchan3_raw: " << current_messages.rc_channels_raw.chan3_raw <<
"\tchan4_raw: " << current_messages.rc_channels_raw.chan4_raw <<
"\tchan5_raw: " << current_messages.rc_channels_raw.chan5_raw <<
"\tchan6_raw: " << current_messages.rc_channels_raw.chan6_raw <<
"\tchan7_raw: " << current_messages.rc_channels_raw.chan7_raw <<
"\tchan8_raw: " << current_messages.rc_channels_raw.chan8_raw <<
"\trssi: " << (int)current_messages.rc_channels_raw.rssi <<
std::endl;
break;
}
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
{
std::cout << "MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:" << std::endl;
mavlink_msg_servo_output_raw_decode(&message, &(current_messages.servo_output_raw));
current_messages.time_stamps.servo_output_raw = get_time_usec();
this_timestamps.servo_output_raw = current_messages.time_stamps.servo_output_raw;
std::cout <<
"\tport: " << (int)current_messages.servo_output_raw.port <<
"\tservo1_raw: " << current_messages.servo_output_raw.servo1_raw <<
"\tservo2_raw: " << current_messages.servo_output_raw.servo2_raw <<
"\tservo3_raw: " << current_messages.servo_output_raw.servo3_raw <<
"\tservo4_raw: " << current_messages.servo_output_raw.servo4_raw <<
"\tservo5_raw: " << current_messages.servo_output_raw.servo5_raw <<
"\tservo6_raw: " << current_messages.servo_output_raw.servo6_raw <<
"\tservo7_raw: " << current_messages.servo_output_raw.servo7_raw <<
"\tservo8_raw: " << current_messages.servo_output_raw.servo8_raw <<
std::endl;
break;
}
case MAVLINK_MSG_ID_VFR_HUD:
{
std::cout << "MAVLINK_MSG_ID_VFR_HUD:" << std::endl;
mavlink_msg_vfr_hud_decode(&message, &(current_messages.vfr_hud));
current_messages.time_stamps.vfr_hud = get_time_usec();
this_timestamps.vfr_hud = current_messages.time_stamps.vfr_hud;
std::cout <<
"\tairspeed: " << current_messages.vfr_hud.airspeed <<
"\tgroundspeed: " << current_messages.vfr_hud.groundspeed <<
"\theading: " << current_messages.vfr_hud.heading <<
std::endl;
std::cout <<
"\tthrottle: " << current_messages.vfr_hud.throttle <<
"\talt: " << current_messages.vfr_hud.alt <<
"\tclimb: " << current_messages.vfr_hud.climb <<
std::endl;
break;
}
case MAVLINK_MSG_ID_MISSION_CURRENT:
{
std::cout << "MAVLINK_MSG_ID_MISSION_CURRENT: ";
mavlink_msg_mission_current_decode(&message, &(current_messages.mission_current));
current_messages.time_stamps.mission_current = get_time_usec();
this_timestamps.mission_current = current_messages.time_stamps.mission_current;
std::cout <<
"seq: " << current_messages.mission_current.seq <<
std::endl;
break;
}
default:
{
printf("Warning, did not handle message id %i\n",message.msgid);
break;
}
} // end: switch msgid
} // end: if read message
// Check for receipt of all items
received_all =
this_timestamps.heartbeat &&
this_timestamps.sys_status &&
// this_timestamps.battery_status &&
// this_timestamps.radio_status &&
this_timestamps.local_position_ned &&
// this_timestamps.global_position_int &&
// this_timestamps.position_target_local_ned &&
this_timestamps.position_target_global_int &&
this_timestamps.highres_imu &&
this_timestamps.attitude ;
// give the write thread time to use the port
if ( writing_status > false )
usleep(100); // look for components of batches at 10kHz
} // end: while not received all
return;
}
/**
* This method parses all incoming bytes and constructs a MAVLink packet.
* It can handle multiple links in parallel, as each link has it's own buffer/
* parsing state machine.
* @param link The interface to read from
* @see LinkInterface
**/
void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
{
// receiveMutex.lock();
mavlink_message_t message;
mavlink_status_t status;
// Cache the link ID for common use.
int linkId = link->getId();
static int mavlink09Count = 0;
static int nonmavlinkCount = 0;
static bool decodedFirstPacket = false;
static bool warnedUser = false;
static bool checkedUserNonMavlink = false;
static bool warnedUserNonMavlink = false;
// FIXME: Add check for if link->getId() >= MAVLINK_COMM_NUM_BUFFERS
for (int position = 0; position < b.size(); position++) {
unsigned int decodeState = mavlink_parse_char(linkId, (uint8_t)(b[position]), &message, &status);
if ((uint8_t)b[position] == 0x55) mavlink09Count++;
if ((mavlink09Count > 100) && !decodedFirstPacket && !warnedUser)
{
warnedUser = true;
// Obviously the user tries to use a 0.9 autopilot
// with QGroundControl built for version 1.0
emit protocolStatusMessage(tr("MAVLink Protocol"), tr("There is a MAVLink Version or Baud Rate Mismatch. "
"Your MAVLink device seems to use the deprecated version 0.9, while QGroundControl only supports version 1.0+. "
"Please upgrade the MAVLink version of your autopilot. "
"If your autopilot is using version 1.0, check if the baud rates of QGroundControl and your autopilot are the same."));
}
if (decodeState == 0 && !decodedFirstPacket)
{
nonmavlinkCount++;
if (nonmavlinkCount > 2000 && !warnedUserNonMavlink)
{
//2000 bytes with no mavlink message. Are we connected to a mavlink capable device?
if (!checkedUserNonMavlink)
{
link->requestReset();
checkedUserNonMavlink = true;
}
else
{
warnedUserNonMavlink = true;
emit protocolStatusMessage(tr("MAVLink Protocol"), tr("There is a MAVLink Version or Baud Rate Mismatch. "
"Please check if the baud rates of QGroundControl and your autopilot are the same."));
}
}
}
if (decodeState == 1)
{
decodedFirstPacket = true;
if (message.msgid == MAVLINK_MSG_ID_PING)
{
// process ping requests (tgt_system and tgt_comp must be zero)
mavlink_ping_t ping;
mavlink_msg_ping_decode(&message, &ping);
if (!ping.target_system && !ping.target_component)
{
mavlink_message_t msg;
mavlink_msg_ping_pack(getSystemId(), getComponentId(), &msg, ping.time_usec, ping.seq, message.sysid, message.compid);
sendMessage(msg);
}
}
if (message.msgid == MAVLINK_MSG_ID_RADIO_STATUS)
{
// process telemetry status message
mavlink_radio_status_t rstatus;
mavlink_msg_radio_status_decode(&message, &rstatus);
emit radioStatusChanged(link, rstatus.rxerrors, rstatus.fixed, rstatus.rssi, rstatus.remrssi,
rstatus.txbuf, rstatus.noise, rstatus.remnoise);
}
// Log data
if (!_logSuspendError && !_logSuspendReplay && _tempLogFile.isOpen()) {
uint8_t buf[MAVLINK_MAX_PACKET_LEN + sizeof(quint64)];
// Write the uint64 time in microseconds in big endian format before the message.
// This timestamp is saved in UTC time. We are only saving in ms precision because
// getting more than this isn't possible with Qt without a ton of extra code.
quint64 time = (quint64)QDateTime::currentMSecsSinceEpoch() * 1000;
qToBigEndian(time, buf);
// Then write the message to the buffer
int len = mavlink_msg_to_send_buffer(buf + sizeof(quint64), &message);
// Determine how many bytes were written by adding the timestamp size to the message size
len += sizeof(quint64);
// Now write this timestamp/message pair to the log.
QByteArray b((const char*)buf, len);
if (_tempLogFile.write(b) != len)
{
// If there's an error logging data, raise an alert and stop logging.
emit protocolStatusMessage(tr("MAVLink Protocol"), tr("MAVLink Logging failed. Could not write to file %1, logging disabled.").arg(_tempLogFile.fileName()));
_stopLogging();
_logSuspendError = true;
}
}
// ORDER MATTERS HERE!
// If the matching UAS object does not yet exist, it has to be created
// before emitting the packetReceived signal
UASInterface* uas = UASManager::instance()->getUASForId(message.sysid);
// Check and (if necessary) create UAS object
if (uas == NULL && message.msgid == MAVLINK_MSG_ID_HEARTBEAT)
{
// ORDER MATTERS HERE!
// The UAS object has first to be created and connected,
// only then the rest of the application can be made aware
// of its existence, as it only then can send and receive
// it's first messages.
// Check if the UAS has the same id like this system
if (message.sysid == getSystemId())
{
emit protocolStatusMessage(tr("MAVLink Protocol"), tr("Warning: A second system is using the same system id (%1)").arg(getSystemId()));
}
// Create a new UAS based on the heartbeat received
// Todo dynamically load plugin at run-time for MAV
// WIKISEARCH:AUTOPILOT_TYPE_INSTANTIATION
// First create new UAS object
// Decode heartbeat message
mavlink_heartbeat_t heartbeat;
// Reset version field to 0
heartbeat.mavlink_version = 0;
mavlink_msg_heartbeat_decode(&message, &heartbeat);
// Check if the UAS has a different protocol version
if (m_enable_version_check && (heartbeat.mavlink_version != MAVLINK_VERSION))
{
// Bring up dialog to inform user
if (!versionMismatchIgnore)
{
emit protocolStatusMessage(tr("MAVLink Protocol"), tr("The MAVLink protocol version on the MAV and QGroundControl mismatch! "
"It is unsafe to use different MAVLink versions. "
"QGroundControl therefore refuses to connect to system %1, which sends MAVLink version %2 (QGroundControl uses version %3).").arg(message.sysid).arg(heartbeat.mavlink_version).arg(MAVLINK_VERSION));
versionMismatchIgnore = true;
}
// Ignore this message and continue gracefully
continue;
}
// Create a new UAS object
uas = QGCMAVLinkUASFactory::createUAS(this, link, message.sysid, &heartbeat);
}
// Increase receive counter
totalReceiveCounter[linkId]++;
currReceiveCounter[linkId]++;
// Determine what the next expected sequence number is, accounting for
// never having seen a message for this system/component pair.
int lastSeq = lastIndex[message.sysid][message.compid];
int expectedSeq = (lastSeq == -1) ? message.seq : (lastSeq + 1);
// And if we didn't encounter that sequence number, record the error
if (message.seq != expectedSeq)
{
// Determine how many messages were skipped
int lostMessages = message.seq - expectedSeq;
// Out of order messages or wraparound can cause this, but we just ignore these conditions for simplicity
if (lostMessages < 0)
{
lostMessages = 0;
}
// And log how many were lost for all time and just this timestep
totalLossCounter[linkId] += lostMessages;
currLossCounter[linkId] += lostMessages;
}
// And update the last sequence number for this system/component pair
lastIndex[message.sysid][message.compid] = expectedSeq;
// Update on every 32th packet
if ((totalReceiveCounter[linkId] & 0x1F) == 0)
{
// Calculate new loss ratio
// Receive loss
float receiveLoss = (double)currLossCounter[linkId] / (double)(currReceiveCounter[linkId] + currLossCounter[linkId]);
receiveLoss *= 100.0f;
currLossCounter[linkId] = 0;
currReceiveCounter[linkId] = 0;
emit receiveLossChanged(message.sysid, receiveLoss);
}
// The packet is emitted as a whole, as it is only 255 - 261 bytes short
// kind of inefficient, but no issue for a groundstation pc.
// It buys as reentrancy for the whole code over all threads
emit messageReceived(link, message);
// Multiplex message if enabled
if (m_multiplexingEnabled)
{
// Get all links connected to this unit
QList<LinkInterface*> links = _linkMgr->getLinks();
// Emit message on all links that are currently connected
foreach(LinkInterface* currLink, links)
{
// Only forward this message to the other links,
// not the link the message was received on
if (currLink != link) sendMessage(currLink, message, message.sysid, message.compid);
}
}
}
}
void MAVLink_Message_Handler::handle_message(uint64_t timestamp, mavlink_message_t &msg)
{
// ::fprintf(stderr, "msg.msgid=%u\n", msg.msgid);
switch(msg.msgid) {
case MAVLINK_MSG_ID_AHRS2: {
mavlink_ahrs2_t decoded;
mavlink_msg_ahrs2_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_ATTITUDE: {
mavlink_attitude_t decoded;
mavlink_msg_attitude_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_EKF_STATUS_REPORT: {
mavlink_ekf_status_report_t decoded;
mavlink_msg_ekf_status_report_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
mavlink_global_position_int_t decoded;
mavlink_msg_global_position_int_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_GPS_RAW_INT: {
mavlink_gps_raw_int_t decoded;
mavlink_msg_gps_raw_int_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_HEARTBEAT: {
mavlink_heartbeat_t decoded;
mavlink_msg_heartbeat_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_MOUNT_STATUS: {
mavlink_mount_status_t decoded;
mavlink_msg_mount_status_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: {
mavlink_nav_controller_output_t decoded;
mavlink_msg_nav_controller_output_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_PARAM_VALUE: {
mavlink_param_value_t decoded;
mavlink_msg_param_value_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK: {
mavlink_remote_log_data_block_t decoded;
mavlink_msg_remote_log_data_block_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_SCALED_PRESSURE: {
mavlink_scaled_pressure_t decoded;
mavlink_msg_scaled_pressure_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_SCALED_PRESSURE2: {
mavlink_scaled_pressure2_t decoded;
mavlink_msg_scaled_pressure2_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: {
mavlink_servo_output_raw_t decoded;
mavlink_msg_servo_output_raw_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_STATUSTEXT: {
mavlink_statustext_t decoded;
mavlink_msg_statustext_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_SYS_STATUS: {
mavlink_sys_status_t decoded;
mavlink_msg_sys_status_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_SYSTEM_TIME: {
mavlink_system_time_t decoded;
mavlink_msg_system_time_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_VFR_HUD: {
mavlink_vfr_hud_t decoded;
mavlink_msg_vfr_hud_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
}
}
/**
* Telemetry transmit task. Processes queue events and periodic updates.
*/
static void telemetryRxTask(void *parameters)
{
uint32_t inputPort;
uint8_t c;
// Task loop
while (1) {
#if defined(PIOS_INCLUDE_USB_HID)
// Determine input port (USB takes priority over telemetry port)
if (PIOS_USB_HID_CheckAvailable(0)) {
inputPort = PIOS_COM_TELEM_USB;
} else
#endif /* PIOS_INCLUDE_USB_HID */
{
inputPort = telemetryPort;
}
mavlink_channel_t mavlink_chan = MAVLINK_COMM_0;
// Block until a byte is available
PIOS_COM_ReceiveBuffer(inputPort, &c, 1, portMAX_DELAY);
// And process it
if (mavlink_parse_char(mavlink_chan, c, &rx_msg, &rx_status))
{
// Handle packet with waypoint component
mavlink_wpm_message_handler(&rx_msg);
// Handle packet with parameter component
mavlink_pm_message_handler(mavlink_chan, &rx_msg);
switch (rx_msg.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
// Check if this is the gcs
mavlink_heartbeat_t beat;
mavlink_msg_heartbeat_decode(&rx_msg, &beat);
if (beat.type == MAV_TYPE_GCS)
{
// Got heartbeat from the GCS, we're good!
lastOperatorHeartbeat = xTaskGetTickCount() * portTICK_RATE_MS;
}
}
break;
case MAVLINK_MSG_ID_SET_MODE:
{
mavlink_set_mode_t mode;
mavlink_msg_set_mode_decode(&rx_msg, &mode);
// Check if this system should change the mode
if (mode.target_system == mavlink_system.sysid)
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
switch (mode.base_mode)
{
case MAV_MODE_MANUAL_ARMED:
{
flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL;
flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED;
}
break;
case MAV_MODE_MANUAL_DISARMED:
{
flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL;
flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
}
break;
case MAV_MODE_PREFLIGHT:
{
flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
}
break;
case MAV_MODE_STABILIZE_ARMED:
{
flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_STABILIZED1;
flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED;
}
break;
case MAV_MODE_GUIDED_ARMED:
{
flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_STABILIZED2;
flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED;
}
break;
case MAV_MODE_AUTO_ARMED:
{
flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_STABILIZED3;
flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED;
}
break;
}
bool newHilEnabled = (mode.base_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL);
if (newHilEnabled != hilEnabled)
{
if (newHilEnabled)
{
// READ-ONLY flag write to ActuatorCommand
UAVObjMetadata meta;
UAVObjHandle handle = ActuatorCommandHandle();
UAVObjGetMetadata(handle, &meta);
meta.access = ACCESS_READONLY;
UAVObjSetMetadata(handle, &meta);
mavlink_missionlib_send_gcs_string("ENABLING HIL SIMULATION");
mavlink_missionlib_send_gcs_string("+++++++++++++++++++++++");
mavlink_missionlib_send_gcs_string("BLOCKING ALL ACTUATORS");
}
else
{
// READ-ONLY flag write to ActuatorCommand
UAVObjMetadata meta;
UAVObjHandle handle = ActuatorCommandHandle();
UAVObjGetMetadata(handle, &meta);
meta.access = ACCESS_READWRITE;
UAVObjSetMetadata(handle, &meta);
mavlink_missionlib_send_gcs_string("DISABLING HIL SIMULATION");
mavlink_missionlib_send_gcs_string("+++++++++++++++++++++++");
mavlink_missionlib_send_gcs_string("ACTIVATING ALL ACTUATORS");
}
}
hilEnabled = newHilEnabled;
FlightStatusSet(&flightStatus);
// Check HIL
bool hilEnabled = (mode.base_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL);
enableHil(hilEnabled);
}
}
break;
case MAVLINK_MSG_ID_HIL_STATE:
{
if (hilEnabled)
{
mavlink_hil_state_t hil;
mavlink_msg_hil_state_decode(&rx_msg, &hil);
// Write GPSPosition
GPSPositionData gps;
GPSPositionGet(&gps);
gps.Altitude = hil.alt/10;
gps.Latitude = hil.lat/10;
gps.Longitude = hil.lon/10;
GPSPositionSet(&gps);
// Write PositionActual
PositionActualData pos;
PositionActualGet(&pos);
// FIXME WRITE POSITION HERE
PositionActualSet(&pos);
// Write AttitudeActual
AttitudeActualData att;
AttitudeActualGet(&att);
att.Roll = hil.roll;
att.Pitch = hil.pitch;
att.Yaw = hil.yaw;
// FIXME
//att.RollSpeed = hil.rollspeed;
//att.PitchSpeed = hil.pitchspeed;
//att.YawSpeed = hil.yawspeed;
// Convert to quaternion formulation
RPY2Quaternion(&attitudeActual.Roll, &attitudeActual.q1);
// Write AttitudeActual
AttitudeActualSet(&att);
// Write AttitudeRaw
AttitudeRawData raw;
AttitudeRawGet(&raw);
raw.gyros[0] = hil.rollspeed;
raw.gyros[1] = hil.pitchspeed;
raw.gyros[2] = hil.yawspeed;
raw.accels[0] = hil.xacc;
raw.accels[1] = hil.yacc;
raw.accels[2] = hil.zacc;
// raw.magnetometers[0] = hil.xmag;
// raw.magnetometers[0] = hil.ymag;
// raw.magnetometers[0] = hil.zmag;
AttitudeRawSet(&raw);
}
}
break;
case MAVLINK_MSG_ID_COMMAND_LONG:
{
// FIXME Implement
}
break;
}
}
}
}
/*Mainloop*/
void loop() {
/*Auswertung des anstehenden Aufgaben im Scheduler*/
int i; unsigned char new_value = 0; unsigned char new_heading = 0;
for (i = 0; i < MAX_TASKS; i++) {
if (scheduler[i].task == NOTHING || get_current_millis() < scheduler[i].tv_msec) {continue;}
switch (scheduler[i].task) {
case NOTHING: break;
case MEASURE:
/*Fordert den als Parameter uebergebenen Sensor auf eine Messung zu starten und bestimmt den Lese-Zeitpunkt*/
srf[scheduler[i].param - SE0_ADDRESS].measure();
schedule_add((long)srf_speed[scheduler[i].param - SE0_ADDRESS], READ_MEASURE, scheduler[i].param);
scheduler[i].task = NOTHING;
break;
case READ_MEASURE: {
unsigned short index = scheduler[i].param - SE0_ADDRESS;
/*Liest die Messdaten des als Parameter uebergebenen Sensors aus und veranlasst erneute Messung*/
srf[index].read_it(get_current_millis());
schedule_add((long)srf[index].get_delay(), MEASURE, scheduler[i].param);
if(state == HOLD_STILL || state == MEASURE_ENVIRONMENT || state == GET_ANCHOR) {
nvalue[index] = 1;
}
/*if (state == HOLD_STILL || state == HEAD_TO_MIDDLE || state = HTM_DELAY) {
nvalue2[scheduler[i].param - SE0_ADDRESS] = 1;
}*/
new_value = scheduler[i].param;
/*Korrektur der Messwerte bei Schieflage des Copters bei glaubhaften Winkeln*/
if ((index == 0 || index == 1) && (abs(current_roll_rad) < MAX_ROLL_ANGLE)) srf[index].set_mean((unsigned short)(srf[index].get_mean()*cos(current_roll_rad)));
else if ((index == 2 || index == 3) && (abs(current_pitch_rad) < MAX_PITCH_ANGLE)) srf[index].set_mean((unsigned short)(srf[index].get_mean()*cos(current_roll_rad)));
/*Korrektur der Messwerte, wenn sich ein Sensor im Nahbereich (< SE_MIN_DISTANCE) befindet*/
static unsigned char rm_state[SE_COUNT];
static short rm_min[SE_COUNT];
if (state != IDLE && state != INIT && state != DELAY) {
if (rm_state[index] == 0 && srf[index].get_mean() < SE_MIN_DISTANCE) {
rm_state[index] = 1;
if (index == 0 || index == 2) rm_min[index] = srf[index+1].get_mean() - SE_MIN_DIFF;
else rm_min[index] = srf[index-1].get_mean() - SE_MIN_DIFF;
srf[index].set_mean(SE_MIN);
} else if (rm_state[index] == 1) {
if ((index == 0 || index == 2) && srf[index+1].get_mean() < rm_min[index]) {
rm_state[index] = 0;
} else if ((index == 1 || index == 3) && srf[index-1].get_mean() < rm_min[index]) {
rm_state[index] = 0;
} else {
srf[index].set_mean(SE_MIN);
}
}
}
if (rm_state[0] == 1 && rm_state[1] == 1) {rm_state[0] = 0; rm_state[1] = 0;}
if (rm_state[2] == 1 && rm_state[3] == 1) {rm_state[2] = 0; rm_state[3] = 0;}
if (state == HOLD_STILL || state == GET_ANCHOR || state == HTM_DELAY) {
if (nvalue2[0] == 1 && nvalue2[1] == 1 && nvalue2[2] == 1 && nvalue2[3] == 1) {
slam_insert_v((srf[0].get_cm_per_s() + srf[1].get_cm_per_s())/2,(srf[2].get_cm_per_s() + srf[3].get_cm_per_s())/2,current_heading,get_current_millis());
nvalue2[0] = 0; nvalue2[1] = 0; nvalue2[2] = 0; nvalue2[3] = 0;
}
}
/*Speichert den gelesenen Messwert in der entsprechenden Log-Datei*/
#if LOG > 0
FILE *fd;
if (scheduler[i].param == SE0_ADDRESS) fd = fd_112;
else if (scheduler[i].param == SE1_ADDRESS) fd = fd_113;
else if (scheduler[i].param == SE2_ADDRESS) fd = fd_114;
else fd = fd_115;
fprintf(fd,"%lu\t%u\t%u\n", srf[scheduler[i].param - SE0_ADDRESS].get_msec(), srf[scheduler[i].param - SE0_ADDRESS].get_data(), srf[scheduler[i].param - SE0_ADDRESS].get_mean());
#endif
scheduler[i].task = NOTHING;
break;
}
case SAVE_LOG:
/*Sichert die bisher geloggten Daten*/
#if LOG > 0
fclose(fd_112);
fclose(fd_113);
fclose(fd_114);
fclose(fd_115);
fclose(fd_data);
char tmp_dir[32]; strcpy(tmp_dir,log_dir);
fd_112 = fopen(strcat(tmp_dir,"/112"), "a"); strcpy(tmp_dir,log_dir);
fd_113 = fopen(strcat(tmp_dir,"/113"), "a"); strcpy(tmp_dir,log_dir);
fd_114 = fopen(strcat(tmp_dir,"/114"), "a"); strcpy(tmp_dir,log_dir);
fd_115 = fopen(strcat(tmp_dir,"/115"), "a"); strcpy(tmp_dir,log_dir);
fd_data= fopen(strcat(tmp_dir,"/data"),"a"); strcpy(tmp_dir,log_dir);
schedule_add(LOG_SPEED,SAVE_LOG,0);
#endif
scheduler[i].task = NOTHING;
break;
case CHECK_STILL:
/*Prueft auf Stillstand des Copters*/
if (still && state == HOLD_STILL) {
if (scheduler[i].param < CHECK_STILL_COUNT) {
schedule_add(CHECK_STILL_SPEED,CHECK_STILL,scheduler[i].param+1);
} else {
state = MEASURE_ENVIRONMENT;
still = 0;
schedule_add(CHECK_STILL_SPEED,CHECK_STILL, 0);
}
} else {
schedule_add(CHECK_STILL_SPEED,CHECK_STILL,0);
}
scheduler[i].task = NOTHING;
break;
case CHANGE_STATE:
/*Ändert den aktuellen Status zum als Parameter übergebenen Status*/
state = (states)scheduler[i].param;
if ((states)scheduler[i].param == HOLD_STILL) hs_state = 0;
scheduler[i].task = NOTHING;
break;
case SHOW_ME: {
/*Schreibt Messwerte und Neigungswinkel auf das Terminal*/
char st[16];
switch (state) {
case INIT: sprintf(st, "INIT"); break;
case MEASURE_ENVIRONMENT: sprintf(st, "ME"); break;
case IDLE: sprintf(st, "IDLE"); break;
case HOLD_STILL: sprintf(st, "HOLD_STILL"); break;
case GET_ALIGNMENT: sprintf(st, "GET_ALIGNMENT"); break;
case GET_ANCHOR: sprintf(st, "GET_ANCHOR"); break;
case DELAY: sprintf(st, "DELAY"); break;
default: sprintf(st, "???"); break;
}
if (roll > 9 || roll < 0) printf("roll: %d\tpitch: %d\tyaw: %d\theading: %d\tdhead: %d\tSE0: %d\tSE1: %d\tSE2: %d\tSE3:%d\tstate: %s\n", roll, pitch, yaw, current_heading, desired_heading, srf[0].get_mean(), srf[1].get_mean(), srf[2].get_mean(), srf[3].get_mean(),st);
else printf("roll: %d\t\tpitch: %d\tyaw: %d\theading: %d\tdhead. %d\tSE0: %d\tSE1: %d\tSE2: %d\tSE3:%d\tstate: %s\n", roll, pitch, yaw, current_heading, desired_heading, srf[0].get_mean(), srf[1].get_mean(), srf[2].get_mean(), srf[3].get_mean(),st);
schedule_add(100,SHOW_ME,0);
scheduler[i].task = NOTHING;
/*Mavlinkkommunikation mit QGroundcontrol (basierend auf Based on http://qgroundcontrol.org/dev/mavlink_linux_integration_tutorial)*/
mavlink_message_t msg;
uint16_t len;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
static int i; i++;
if (i == 10) {
i = 0;
mavlink_msg_heartbeat_pack(0, 0, &msg, 0, 0, 0, 0, 0);
len = mavlink_msg_to_send_buffer(buf, &msg);
sendto(s, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
}
mavlink_msg_debug_vect_pack(0,0,&msg,"DEBUG",0,roll,srf[0].get_mean(),srf[0].get_data());
len = mavlink_msg_to_send_buffer(buf, &msg);
sendto(s, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
break;
}
default: if(WARNINGS){perror("LOOP: Unbekannte Aufgabe im Scheduler.");} break;
}
}
/*Auswertung der Daten, die an der seriellen Schnittstelle anliegen.*/
struct timeval tv; tv.tv_sec = 0; tv.tv_usec = 0;
/*Prueft, ob Daten an der seriellen Schnittstelle anliegen*/
FD_ZERO(&tty_fdset);
FD_SET(tty_fd, &tty_fdset);
if (select(tty_fd+1, &tty_fdset, NULL, NULL, &tv) == -1) {perror("LOOP: select() fehlgeschlagen"); exit(1);}
/*Liest ein einzelnes Byte von der seriellen Schnittstelle und prueft, ob ein Mavlinkpaket vervollstaendigt wurde*/
if (FD_ISSET(tty_fd, &tty_fdset)) {
static mavlink_message_t msg;
static mavlink_status_t status;
char c[1];
if (read(tty_fd,c,1) == -1) {if(WARNINGS){perror("LOOP: Fehler beim Lesen aus " TTY_DEVICE);}}
#if DEBUG_LEVEL > 2
printf("%#x\n",c[0]);
#endif
if (mavlink_parse_char(MAVLINK_COMM_0,(uint8_t) *c, &msg, &status)) {
#if DEBUG_LEVEL > 1
printf("%u Mavlinkpaket empfangen: %d\n", get_current_millis(), msg.msgid);
#endif
//printf("%lu Mavlinkpaket empfangen: %d\n", get_current_millis(), msg.msgid);
switch (msg.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT:
/*Beim Empfang des ersten Heartbeats wird zunaechst ein eventuell vorhandener Datenstream gekuendigt und ein neuer Datenstream abonnemiert*/
mavlink_heartbeat_t hb;
mavlink_msg_heartbeat_decode(&msg,&hb);
if (!first_heartbeat) {
first_heartbeat = 1;
request_data_stream(MAV_DATA_STREAM_ALL,0,0);
}
//if (state == IDLE) {
if (state == IDLE && (hb.custom_mode == 13 /*EXT_CTRL*/ || desktop_build)) {
std::cout << "State changed from IDLE to INIT.\n";
state = INIT;
schedule_add(2000,CHANGE_STATE,(int)HOLD_STILL);
request_data_stream(MAV_DATA_STREAM_EXTRA2/*VFR_HUD*/,DATA_STREAM_SPEED,1);
init_state = 1;
}
break;
case MAVLINK_MSG_ID_VFR_HUD:
/*Speichert bei Empfang von HUD-Daten die aktuelle Ausrichtung des Copters*/
mavlink_vfr_hud_t hud_data;
mavlink_msg_vfr_hud_decode(&msg, &hud_data);
old_heading = current_heading;
current_heading = hud_data.heading;
new_heading = 1;
break;
case MAVLINK_MSG_ID_ATTITUDE:
mavlink_attitude_t adata;
mavlink_msg_attitude_decode(&msg,&adata);
old_heading = current_heading;
current_heading = ((short)lround(DEG(adata.yaw))+360)%360;
current_pitch_rad = adata.pitch;
current_roll_rad = adata.roll;
new_heading = 1;
break;
case MAVLINK_MSG_ID_RAW_IMU:
/*Speichert die Beschleunigungen auf der x- und y-Achse*/
mavlink_raw_imu_t raw_imu;
mavlink_msg_raw_imu_decode(&msg,&raw_imu);
xacc = raw_imu.xacc;
yacc = raw_imu.yacc;
//slam_insert_acc(xacc,yacc,current_heading,get_current_millis());
std::cout << "xacc: " << xacc << " yacc: " << yacc << "\n";
break;
case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
/*Prüft, ob eine Umstellung auf externe Kontrolle erfolgt ist*/
/*if (state != IDLE) {break;}
mavlink_rc_channels_raw_t rc;
mavlink_msg_rc_channels_raw_decode(&msg,&rc);
if (desktop_build) init_state = 1; rc.chan5_raw = MODE_SWITCH_RANGE_UP-1;
if (init_state && rc.chan5_raw < MODE_SWITCH_RANGE_UP && rc.chan5_raw > MODE_SWITCH_RANGE_DOWN) {
std::cout << "State changed from IDLE to INIT.\n";
schedule_add(0,CHANGE_STATE,(int)INIT);
schedule_add(2000,CHANGE_STATE,(int)HOLD_STILL);
request_data_stream(MAV_DATA_STREAM_RC_CHANNELS,0,0);
request_data_stream(MAV_DATA_STREAM_EXTRA2,DATA_STREAM_SPEED,1);
request_data_stream(MAV_DATA_STREAM_RAW_SENSORS,DATA_STREAM_SPEED,1);
}
if(rc.chan5_raw > MODE_SWITCH_RANGE_UP || rc.chan5_raw < MODE_SWITCH_RANGE_DOWN) {
init_state = 1;
}*/
break;
default: break;
}
}
}
/*Berechnung der vorgeschlagenen Werte fuer roll, pitch und yaw an Hand der neuen Messwerte*/
if (state != INIT && !new_value && !new_heading) {/*Wenn keine neuen Daten vorhanden sind -> Abarbeitung überspringen*/return;}
switch (state) {
case IDLE: /*Wartet auf Aktivierung der externen Kontrolle*/ break;
case INIT:
std::cout << "INIT\n";
/*Initialisiert Abarbeitung, wenn externe Kontrolle zum ersten Mal aktiviert wurde*/
schedule_add(5000,CHECK_STILL,0);
#if LOG > 0
schedule_add(LOG_SPEED,SAVE_LOG,0);
//schedule_add(SLAM_SPEED,SAVE_SLAM,0);
#endif
for (int i = 0; i < SE_COUNT; i++) {
if ((int)pow(2,i) & ENABLED_SENSORS) {
schedule_add(0, MEASURE, SE0_ADDRESS + i);
}
}
slam_init(current_heading,get_current_millis());
state = DELAY;
break;
case HOLD_STILL:
if(1) {
static short set_point[SE_COUNT];
if (hs_state == 0) {
hs_state = 1;
set_point[0] = (srf[0].get_mean() + srf[1].get_mean())/2;
set_point[1] = set_point[0];
set_point[2] = (srf[2].get_mean() + srf[3].get_mean())/2;
set_point[3] = set_point[2];
//for (int i = 0; i < SE_COUNT; i++) {
// set_point[i] = srf[i].get_mean();
// std::cout << "Set_Point " << i << ":" << set_point[i] << "\n";
//}
/*for (int i = 0; i < SE_COUNT; i++) {
if (set_point[i] < 100 || set_point[i] > 0) {
if (i == 0 || i == 2) {
set_point[i+1] -= (100 - set_point[i]);
} else {
set_point[i-1] -= (100 - set_point[i]);
}
set_point[i] = 100;
}
}*/
for (int i = 0; i < SE_COUNT; i++) {
std::cout << "Set_Point " << i << ":" << set_point[i] << "\n";
}
}
/*Veranlasst den Copter auf Grundlage der Änderungen der Messwerte still zu stehen*/
if (!new_value) break;
if (nvalue[0] == 1 && nvalue[1] == 1) {
if (srf[0].get_mean() == SE_MIN) roll = between<short>(pid_roll.get(0 - srf[1].get_mean() + set_point[1],srf[1].get_msec_diff()),-max_roll,max_roll);
else if (srf[1].get_mean() == SE_MIN) roll = between<short>(pid_roll.get(srf[0].get_mean() - set_point[0],srf[0].get_msec_diff()),-max_roll,max_roll);
else roll = between<short>(pid_roll.get(((srf[0].get_mean() - set_point[0]) - (srf[1].get_mean() - set_point[1]))/2,(srf[0].get_msec_diff() + srf[1].get_msec_diff())/2),-max_roll,max_roll);
nvalue[0] = 0; nvalue[1] = 0;
}
if (nvalue[2] == 1 && nvalue[3] == 1) {
if (srf[2].get_mean() == SE_MIN) pitch = between<short>(pid_pitch.get(0 - srf[3].get_mean() + set_point[3],srf[3].get_msec_diff()),-max_pitch,max_pitch);
else if (srf[3].get_mean() == SE_MIN) pitch = between<short>(pid_pitch.get(srf[2].get_mean() - set_point[2],srf[2].get_msec_diff()),-max_pitch,max_pitch);
else pitch = between<short>(pid_pitch.get(((srf[2].get_mean() - set_point[2]) - (srf[3].get_mean() - set_point[3]))/2,(srf[2].get_msec_diff() + srf[3].get_msec_diff())/2),-max_pitch,max_pitch);
nvalue[2] = 0; nvalue[3] = 0;
}
yaw = 0;
send_ext_ctrl();
/*Prüfung auf Stillstand*/
if (abs(roll) <= STILL_FAK_ROLL*max_roll/100 && abs(pitch) <= STILL_FAK_PITCH*max_pitch/100) {
//still = 1;
still = 0; /*FIXME*/
} else {
still = 0;
}
}
break;
case MEASURE_ENVIRONMENT:
/*Vermisst die Umgebung durch eine Drehung um 360/SE_COUNT Grad*/
if (breakpoint == 1) {state = HOLD_STILL; hs_state= 0; break;}
if (abs(xacc) > HOLD_STILL_MAX_XACC || abs(yacc) > HOLD_STILL_MAX_YACC) {/*Wurde der Copter zu stark bewegt: Abbruch*/state = HOLD_STILL; hs_state = 0; std::cout << "State changed to HOLD_STILL\n"; break;}
if (first_heading == -1) {
/*Speichert die anfaengliche Ausrichtung und veranlasst den Copter sich zu drehen*/
std::cout << "State changed to MEASURE_ENVIRONMENT\n";
first_heading = current_heading;
roll = 0; pitch = 0; rotation = 0; yaw = rotation_angle_sign*CONST_YAW;
send_ext_ctrl();
}
//if (new_value) slam_insert_measurement(srf[new_value-SE0_ADDRESS].get_mean(),(current_heading + (360/SE_COUNT)*(new_value-SE0_ADDRESS))%360);
/*Berechnung der aktuellen Drehung*/
if (new_heading) {
if (abs(current_heading - old_heading) > 180/SE_COUNT) rotation += current_heading - old_heading - sign(current_heading - old_heading)*360;
else rotation += current_heading - old_heading;
for(int i = 0; i < SE_COUNT; i++) {
if(nvalue[i] == 1) {
env[(current_heading + srf[i].get_alignment()*(360/SE_COUNT))%360] = srf[i].get_mean();
nvalue[i] = 0;
}
}
//if (abs(rotation) >= 360/SE_COUNT) {
if (abs(rotation) >= rotation_angle) {
/*Copter hat sich ausreichend gedreht, Abbruch der Vermessung*/
//state = HEAD_TO_MIDDLE;
state = GET_ALIGNMENT;
roll = 0; pitch = 0; yaw = 0;
send_ext_ctrl();
rotation_angle_sign = sign(rotation);
}
}
break;
case GET_ALIGNMENT: {
/*Bestimme für jeden Sensor die Minimalentfernung der letzten Drehung*/
/*short min_v[SE_COUNT];
short min_h[SE_COUNT];
for (int i = 0; i < SE_COUNT; i++) {
min_v[i] = 0;
min_h[i] = 0;
}*/
short min_v = 0;
for (int i = 0; i < rotation_angle; i++) {
if (min_v < env[(first_heading + rotation_angle_sign*i + 360 + srf[align_se].get_alignment()*(360/SE_COUNT))%360]) {
min_v = env[(first_heading + rotation_angle_sign*i + 360 + srf[align_se].get_alignment()*(360/SE_COUNT))%360];
desired_heading = (first_heading + rotation_angle_sign*i + 360 + srf[align_se].get_alignment()*(360/SE_COUNT))%360;
}
/*for (int j = 0; j < SE_COUNT; j++) {
if (min_v[j] < env[(first_heading + rotation_angle_sign*i + j*360/SE_COUNT + 360)%360]) {
min_v[j] = env[(first_heading + rotation_angle_sign*i + j*360/SE_COUNT + 360)%360];
min_h[j] = (first_heading + rotation_angle_sign*i + 360)%360;
}
}*/
}
//desired_heading = round((float)(min_h[0]+min_h[1]+min_h[2]+min_h[3])/4);
/*Bestimmung der künftigen Drehrichtung*/
for (int k = 0; k < rotation_angle/2; k++) {
/*Wenn Minimum in der ersten Hälfte der Drehung gefunden wurde, Wechsel der Drehrichtung bzw. Abbruch*/
if (desired_heading == (first_heading+rotation_angle_sign*k)%360) {
rotation_angle_sign *= -1;
if (init_state == 2) init_state = 3;
break;
}
}
/*Cleanup*/
first_heading = -1; yaw = 0; pid_roll.reset(); pid_pitch.reset();
for(int i = 0; i < 360; i++) env[i] = 0;
if (init_state == 1) init_state = 2;
if (init_state == 3) {
state = GET_ANCHOR;
pid_yaw.reset();
pid_yaw.set(HTM_YAW_KP,HTM_YAW_TN,HTM_YAW_TV);
pid_yaw.set_target(0);
roll = 0; pitch = 0; yaw = 0;
std::cout << "State changed to GET_ANCHOR\n";
tv_old_heading = get_current_millis();
} else {
state = HOLD_STILL;
hs_state = 0;
}
break;
}
case GET_ANCHOR:
/*Copter versucht sich stabil auf ANCHOR_DISTANCE und desired_heading zu stellen*/
if (new_heading) {
short heading_diff;
/*Berechnung der Differenz zwischen aktueller Ausrichtung und gewünschter Ausrichtung*/
if (abs(desired_heading - current_heading) > 180) {
heading_diff = desired_heading - current_heading - sign(desired_heading - current_heading)*360;
} else {
heading_diff = desired_heading - current_heading;
}
yaw = pid_yaw.get(heading_diff,get_current_millis() - tv_old_heading);
tv_old_heading = get_current_millis();
}
if (new_value) {
roll = 0; pitch = 0;
if (nvalue[align_se] == 1) {
if (align_se == 0) roll = between<short>(pid_roll_anc.get(srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_roll, max_roll);
else if (align_se == 1) roll = between<short>(pid_roll_anc.get((-1)*srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_roll, max_roll);
else if (align_se == 2) pitch = between<short>(pid_pitch_anc.get(srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_pitch, max_pitch);
else if (align_se == 3) pitch = between<short>(pid_pitch_anc.get((-1)*srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_pitch, max_pitch);
nvalue[align_se] = 0;
}
if (srf[0].get_mean() < 30) roll = 1000;
else if (srf[1].get_mean() < 30) roll = -1000;
else if (srf[2].get_mean() < 30) pitch = 1000;
else if (srf[3].get_mean() < 30) pitch = -1000;
if (abs(srf[0].get_mean() - ANCHOR_DISTANCE) < 5) {
for (int i = 0; i < SE_COUNT; i++) nvalue[i] = 0;
/*state = MOVE_BACKWARD*/
/*TODO Neuausrichtung des Copters?*/
}
}
if (breakpoint == 3) {roll = 0; pitch = 0; send_ext_ctrl();}
else if (breakpoint != 2) {send_ext_ctrl();}
break;
case DELAY: break;
case HTM_DELAY: break;
default: break;
}
#if LOG > 0
fprintf(fd_data,"%lu\t%d\t%d\t%d\t%d\t%d\t%f\t%f\n", (unsigned long)get_current_millis(), roll, pitch, yaw, current_heading, state, current_roll_rad, current_pitch_rad);
#endif
}
static void downstream_handle_heartbeat(mavlink_message_t* msg) {
mavlink_heartbeat_t pkt;
mavlink_msg_heartbeat_decode(msg, &pkt);
sm.on_downstream_heartbeat(&pkt);
}
void*
_mavlink_recv_thread(void *ptr) {
unsigned char buf[MAVLINK_MAX_PACKET_LEN];
rctl_link_t *link = (rctl_link_t*)ptr;
fprintf(stdout, "recv thread started...\n");
while (mrt_running) {
ssize_t r = recv(link->ml_sock, buf, MAVLINK_MAX_PACKET_LEN, 0);
if ((r == -1) && (errno != EAGAIN && errno != EWOULDBLOCK))
fprintf(stderr, "recv error: %d\n", errno);
else {
// handle message
uint8_t link_id = 0;
mavlink_message_t msg;
mavlink_status_t status;
unsigned int decode_state;
for (ssize_t i = 0; i < r; i++) {
decode_state = mavlink_parse_char(link_id, (uint8_t)(buf[i]), &msg, &status);
if (decode_state == 1) {
// mavlink_highres_imu_t imu;
mavlink_heartbeat_t hb;
// mavlink_sys_status_t sys;
switch (msg.msgid) {
/*
case MAVLINK_MSG_ID_HIGHRES_IMU:
mavlink_msg_highres_imu_decode(&msg, &imu);
// printf("IMU Data. Accel (%4.4f, %4.4f, %4.4f)\n", imu.xacc, imu.yacc, imu.zacc);
break;
*/
case MAVLINK_MSG_ID_HEARTBEAT:
mavlink_msg_heartbeat_decode(&msg, &hb);
// printf("Heartbeat. Base Mode: %u Custom Mode: %u\n", hb.base_mode, hb.custom_mode);
// TODO: mutexes?
// printf("HEARTBEAT %d\n", msg.msgid);
baseMode = hb.base_mode;
navMode = hb.custom_mode;
break;
/*
case MAVLINK_MSG_ID_SYS_STATUS:
mavlink_msg_sys_status_decode(&msg, &sys);
printf("System Status. Battery Voltage: %u [mV] (%d %% remaining)\n", sys.voltage_battery, sys.battery_remaining);
break;
*/
default:
// printf("Unhandled msgid %d\n", msg.msgid);
break;
}
// invoke callback function
if (link->cfg->mavlink_handler)
link->cfg->mavlink_handler(msg);
}
}
}
}
return NULL;
}
void protDecodeMavlink(void) {
uint8_t indx, writeSuccess, commChannel = 1;
mavlink_param_set_t set;
mavlink_message_t msg;
mavlink_status_t status;
// uint8_t* dataIn;
// fix the data length so if the interrupt adds data
// during execution of this block, it will be read
// until the next gsRead
unsigned int tmpLen = getLength(uartMavlinkInBuffer), i = 0;
// if the buffer has more data than the max size, set it to max,
// otherwise set it to the length
//DatafromGSmavlink[0] = (tmpLen > MAXINLEN) ? MAXINLEN : tmpLen;
// read the data
//for (i = 1; i <= DatafromGSmavlink[0]; i += 1) {
//mavlink_parse_char(commChannel, readFront(uartBufferInMavlink), &msg, &status);
//DatafromGSmavlink[i] = readFront(uartMavlinkInBuffer);
//}
//dataIn = DatafromGSmavlink;
// increment the age of heartbeat
mlPending.heartbeatAge++;
for (i = 0; i <= tmpLen; i++) {
// Try to get a new message
if (mavlink_parse_char(commChannel, readFront(uartMavlinkInBuffer), &msg, &status)) {
// Handle message
switch (msg.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT:
mavlink_msg_heartbeat_decode(&msg, &mlHeartbeat);
// Reset the heartbeat
mlPending.heartbeatAge = 0;
break;
//AM DBG
case MAVLINK_MSG_ID_MISSION_COUNT:
if (!mlPending.wpTransaction && (mlPending.wpProtState == WP_PROT_IDLE)) {
mavlink_msg_mission_count_decode(&msg, &mlWpCount);
// Start the transaction
mlPending.wpTransaction = 1;
// change the state
mlPending.wpProtState = WP_PROT_GETTING_WP_IDLE;
// reset the rest of the state machine
mlPending.wpTotalWps = mlWpCount.count;
mlPending.wpCurrentWpInTransaction = 0;
mlPending.wpTimeOut = 0;
}
break;
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
// if there is no transaction going on
if (!mlPending.wpTransaction && (mlPending.wpProtState == WP_PROT_IDLE)) {
// Start the transaction
mlPending.wpTransaction = 1;
// change the state
mlPending.wpProtState = WP_PROT_LIST_REQUESTED;
// reset the rest of the state machine
mlPending.wpCurrentWpInTransaction = 0;
mlPending.wpTimeOut = 0;
}
break;
case MAVLINK_MSG_ID_MISSION_REQUEST:
mavlink_msg_mission_request_decode(&msg, &mlWpRequest);
if (mlPending.wpTransaction && (mlWpRequest.seq < mlWpValues.wpCount)) {
// change the state
mlPending.wpProtState = WP_PROT_TX_WP;
// reset the rest of the state machine
mlPending.wpCurrentWpInTransaction = mlWpRequest.seq;
mlPending.wpTimeOut = 0;
} else {
// TODO: put here a report for a single WP, i.e. not inside a transaction
}
break;
case MAVLINK_MSG_ID_MISSION_ACK:
mavlink_msg_mission_ack_decode(&msg, &mlWpAck);
if (mlPending.wpTransaction) {
// End the transaction
mlPending.wpTransaction = 0;
// change the state
mlPending.wpProtState = WP_PROT_IDLE;
// reset the rest of the state machine
mlPending.wpCurrentWpInTransaction = 0;
mlPending.wpTimeOut = 0;
// send current waypoint index
mlPending.wpSendCurrent = TRUE;
}
break;
case MAVLINK_MSG_ID_MISSION_ITEM:
writeSuccess = SUCCESS;
mavlink_msg_mission_item_decode(&msg, &mlSingleWp);
if (mlPending.wpTransaction && (mlPending.wpProtState == WP_PROT_RX_WP)) {
mlPending.wpProtState = WP_PROT_GETTING_WP_IDLE;
}
indx = (uint8_t) mlSingleWp.seq;
mlWpValues.lat[indx] = mlSingleWp.x;
mlWpValues.lon[indx] = mlSingleWp.y;
mlWpValues.alt[indx] = mlSingleWp.z;
mlWpValues.type[indx] = mlSingleWp.command;
mlWpValues.orbit[indx] = (uint16_t) mlSingleWp.param3;
/*
// Record the data in EEPROM
writeSuccess = storeWaypointInEeprom(&mlSingleWp);
// Set the flag of Aknowledge for the AKN Message
// if the write was not successful
if (writeSuccess != SUCCESS) {
mlPending.wpAck++;
mlWpAck.target_component = MAV_COMP_ID_MISSIONPLANNER;
mlWpAck.type = MAV_MISSION_ERROR;
}
*/
break;
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
writeSuccess = SUCCESS;
// clear the WP values in memory;
memset(&mlWpValues, 0, sizeof (mavlink_mission_item_values_t));
/*
writeSuccess = clearWaypointsFrom(0);
// Set the flag of Aknowledge fail
// if the write was unsuccessful
if (writeSuccess != SUCCESS) {
mlPending.statustext++;
mlStatustext.severity = MAV_SEVERITY_ERROR;
strncpy(mlStatustext.text, "Failed to clear waypoints from EEPROM.", 49);
}
*/
// Update the waypoint count
mlWpValues.wpCount = 0;
// Set the state machine ready to send the WP akn
mlPending.wpCurrentWpInTransaction = 0;
mlPending.wpTotalWps = 0;
mlPending.wpTransaction = 1;
mlPending.wpProtState = WP_PROT_GETTING_WP_IDLE;
break;
case MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN:
writeSuccess = SUCCESS;
memset(&mlSingleWp, 0, sizeof (mavlink_mission_item_t));
mavlink_msg_set_gps_global_origin_decode(&msg, &mlGSLocation);
mlSingleWp.x = (float) (mlGSLocation.latitude);
mlSingleWp.y = (float) (mlGSLocation.longitude);
mlSingleWp.z = (float) (mlGSLocation.altitude);
indx = (uint8_t) MAX_NUM_WPS - 1;
mlWpValues.lat[indx] = mlSingleWp.x;
mlWpValues.lon[indx] = mlSingleWp.y;
mlWpValues.alt[indx] = mlSingleWp.z;
mlWpValues.type[indx] = MAV_CMD_NAV_LAND;
mlWpValues.orbit[indx] = 0;
// Record the data in EEPROM
/*
writeSuccess = storeWaypointInEeprom(&mlSingleWp);
if (writeSuccess != SUCCESS) {
mlPending.statustext++;
mlStatustext.severity = MAV_SEVERITY_ERROR;
strncpy(mlStatustext.text, "Failed to write origin to EEPROM.", 49);
}
else {
mlPending.statustext++;
mlStatustext.severity = MAV_SEVERITY_INFO;
strncpy(mlStatustext.text, "Control DSC GPS origin set.", 49);
}
*/
break;
//AM DBG
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
mlPending.piTransaction = 1;
mlPending.piProtState = PI_SEND_ALL_PARAM;
mlPending.piCurrentParamInTransaction = 0;
break;
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
// If it was in the middle of a list transmission or there is already a param enqueued
mlPending.piTransaction = 1;
switch (mlPending.piProtState) {
case PI_IDLE:
mlPending.piBackToList = 0; // no need to go back
mlPending.piQIdx = -1; // no Index
mlPending.piCurrentParamInTransaction = mavlink_msg_param_request_read_get_param_index(&msg); // assign directly
mlPending.piProtState = PI_SEND_ONE_PARAM;
break;
case PI_SEND_ALL_PARAM:
mlPending.piBackToList = 1; // mark to go back
mlPending.piQIdx++; // done like this because when empty index = -1
mlPending.piQueue[mlPending.piQIdx] = mavlink_msg_param_request_read_get_param_index(&msg); // put in in queue
mlPending.piProtState = PI_SEND_ONE_PARAM;
break;
case PI_SEND_ONE_PARAM:
if (mlPending.piBackToList) {
mlPending.piQIdx++; // done like this because when empty index = -1
mlPending.piQueue[mlPending.piQIdx] = mavlink_msg_param_request_read_get_param_index(&msg); // put in in queue
}
mlPending.piProtState = PI_SEND_ONE_PARAM;
break;
}
break;
case MAVLINK_MSG_ID_PARAM_SET:
mavlink_msg_param_set_decode(&msg, &set);
if ((uint8_t) set.target_system == (uint8_t) SYSTEMID &&
(uint8_t) set.target_component == (uint8_t) COMPID) {
char* key = (char*) set.param_id;
uint8_t i, j;
uint8_t match;
for (i = 0; i < PAR_PARAM_COUNT; i++) {
match = 1;
for (j = 0; j < PARAM_NAME_LENGTH; j++) {
// Compare
if (((char) (mlParamInterface.param_name[i][j]))
!= (char) (key[j])) {
match = 0;
} // if
// End matching if null termination is reached
if (((char) mlParamInterface.param_name[i][j]) == '\0') {
break;
} // if
}// for j
// Check if matched
if (match) {
//sw_debug = 1;
// Only write and emit changes if there is actually a difference
// AND only write if new value is NOT "not-a-number"
// AND is NOT infinity
if (isFinite(set.param_value)) {
mlParamInterface.param[i] = set.param_value;
// Report back new value
mlPending.piBackToList = 0; // no need to go back
mlPending.piQIdx = -1; // no Index
mlPending.piCurrentParamInTransaction = i; // assign directly
mlPending.piProtState = PI_SEND_ONE_PARAM;
mlPending.piTransaction = 1;
} // if different and not nan and not inf
} // if match
}// for i
} // if addressed to this
break;
default:
break;
}
}
}
}
// ------------------------------------------------------------------------------
// Read Messages
// ------------------------------------------------------------------------------
void
Autopilot_Interface::
read_messages()
{
bool success; // receive success flag
bool received_all = false; // receive only one message
Time_Stamps this_timestamps;
// Blocking wait for new data
while ( not received_all and not time_to_exit )
{
// ----------------------------------------------------------------------
// READ MESSAGE
// ----------------------------------------------------------------------
mavlink_message_t message;
success = serial_port->read_message(message);
// ----------------------------------------------------------------------
// HANDLE MESSAGE
// ----------------------------------------------------------------------
if( success )
{
// Store message sysid and compid.
// Note this doesn't handle multiple message sources.
current_messages.sysid = message.sysid;
current_messages.compid = message.compid;
// Handle Message ID
switch (message.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
//printf("MAVLINK_MSG_ID_HEARTBEAT\n");
mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));
current_messages.time_stamps.heartbeat = get_time_usec();
this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
break;
}
case MAVLINK_MSG_ID_SYS_STATUS:
{
//printf("MAVLINK_MSG_ID_SYS_STATUS\n");
mavlink_msg_sys_status_decode(&message, &(current_messages.sys_status));
current_messages.time_stamps.sys_status = get_time_usec();
this_timestamps.sys_status = current_messages.time_stamps.sys_status;
break;
}
case MAVLINK_MSG_ID_BATTERY_STATUS:
{
//printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
mavlink_msg_battery_status_decode(&message, &(current_messages.battery_status));
current_messages.time_stamps.battery_status = get_time_usec();
this_timestamps.battery_status = current_messages.time_stamps.battery_status;
break;
}
case MAVLINK_MSG_ID_RADIO_STATUS:
{
//printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
mavlink_msg_radio_status_decode(&message, &(current_messages.radio_status));
current_messages.time_stamps.radio_status = get_time_usec();
this_timestamps.radio_status = current_messages.time_stamps.radio_status;
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
{
//printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned));
current_messages.time_stamps.local_position_ned = get_time_usec();
this_timestamps.local_position_ned = current_messages.time_stamps.local_position_ned;
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
//printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
mavlink_msg_global_position_int_decode(&message, &(current_messages.global_position_int));
current_messages.time_stamps.global_position_int = get_time_usec();
this_timestamps.global_position_int = current_messages.time_stamps.global_position_int;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
{
//printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
mavlink_msg_position_target_local_ned_decode(&message, &(current_messages.position_target_local_ned));
current_messages.time_stamps.position_target_local_ned = get_time_usec();
this_timestamps.position_target_local_ned = current_messages.time_stamps.position_target_local_ned;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
{
//printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
mavlink_msg_position_target_global_int_decode(&message, &(current_messages.position_target_global_int));
current_messages.time_stamps.position_target_global_int = get_time_usec();
this_timestamps.position_target_global_int = current_messages.time_stamps.position_target_global_int;
break;
}
case MAVLINK_MSG_ID_HIGHRES_IMU:
{
//printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu));
current_messages.time_stamps.highres_imu = get_time_usec();
this_timestamps.highres_imu = current_messages.time_stamps.highres_imu;
break;
}
case MAVLINK_MSG_ID_ATTITUDE:
{
//printf("MAVLINK_MSG_ID_ATTITUDE\n");
mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
current_messages.time_stamps.attitude = get_time_usec();
this_timestamps.attitude = current_messages.time_stamps.attitude;
break;
}
default:
{
// printf("Warning, did not handle message id %i\n",message.msgid);
break;
}
} // end: switch msgid
} // end: if read message
// Check for receipt of all items
received_all =
this_timestamps.heartbeat &&
this_timestamps.sys_status &&
// this_timestamps.battery_status &&
// this_timestamps.radio_status &&
this_timestamps.local_position_ned &&
// this_timestamps.global_position_int &&
// this_timestamps.position_target_local_ned &&
this_timestamps.position_target_global_int &&
this_timestamps.highres_imu &&
this_timestamps.attitude ;
// give the write thread time to use the port
if ( writing_status > false )
usleep(100); // look for components of batches at 10kHz
} // end: while not received all
return;
}
//0
void MavSerialPort::heartbeat_handler(){
mavlink_msg_heartbeat_decode(&message, &heartbeat);
emit heartbeatReceived();
}
void _MavlinkInterface::handleMessages()
{
Time_Stamps this_timestamps;
mavlink_message_t message;
int nMsgHandled;
nMsgHandled = 0;
//Handle Message while new message is received
while (readMessage(message))
{
// Note this doesn't handle multiple message sources.
current_messages.sysid = message.sysid;
current_messages.compid = message.compid;
system_id = current_messages.sysid;
autopilot_id = current_messages.compid;
// Handle Message ID
switch (message.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
// printf("MAVLINK_MSG_ID_HEARTBEAT\n");
mavlink_msg_heartbeat_decode(&message,
&(current_messages.heartbeat));
current_messages.time_stamps.heartbeat = get_time_usec();
this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
break;
}
case MAVLINK_MSG_ID_SYS_STATUS:
{
// printf("MAVLINK_MSG_ID_SYS_STATUS\n");
mavlink_msg_sys_status_decode(&message,
&(current_messages.sys_status));
current_messages.time_stamps.sys_status = get_time_usec();
this_timestamps.sys_status =
current_messages.time_stamps.sys_status;
break;
}
case MAVLINK_MSG_ID_BATTERY_STATUS:
{
// printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
mavlink_msg_battery_status_decode(&message,
&(current_messages.battery_status));
current_messages.time_stamps.battery_status = get_time_usec();
this_timestamps.battery_status =
current_messages.time_stamps.battery_status;
break;
}
case MAVLINK_MSG_ID_RADIO_STATUS:
{
// printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
mavlink_msg_radio_status_decode(&message,
&(current_messages.radio_status));
current_messages.time_stamps.radio_status = get_time_usec();
this_timestamps.radio_status =
current_messages.time_stamps.radio_status;
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
{
// printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
mavlink_msg_local_position_ned_decode(&message,
&(current_messages.local_position_ned));
current_messages.time_stamps.local_position_ned = get_time_usec();
this_timestamps.local_position_ned =
current_messages.time_stamps.local_position_ned;
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
// printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
mavlink_msg_global_position_int_decode(&message,
&(current_messages.global_position_int));
current_messages.time_stamps.global_position_int = get_time_usec();
this_timestamps.global_position_int =
current_messages.time_stamps.global_position_int;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
{
// printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
mavlink_msg_position_target_local_ned_decode(&message,
&(current_messages.position_target_local_ned));
current_messages.time_stamps.position_target_local_ned =
get_time_usec();
this_timestamps.position_target_local_ned =
current_messages.time_stamps.position_target_local_ned;
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
{
// printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
mavlink_msg_position_target_global_int_decode(&message,
&(current_messages.position_target_global_int));
current_messages.time_stamps.position_target_global_int =
get_time_usec();
this_timestamps.position_target_global_int =
current_messages.time_stamps.position_target_global_int;
break;
}
case MAVLINK_MSG_ID_HIGHRES_IMU:
{
// printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
mavlink_msg_highres_imu_decode(&message,
&(current_messages.highres_imu));
current_messages.time_stamps.highres_imu = get_time_usec();
this_timestamps.highres_imu =
current_messages.time_stamps.highres_imu;
break;
}
case MAVLINK_MSG_ID_ATTITUDE:
{
// printf("MAVLINK_MSG_ID_ATTITUDE\n");
mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
current_messages.time_stamps.attitude = get_time_usec();
this_timestamps.attitude = current_messages.time_stamps.attitude;
break;
}
default:
{
// printf("Warning, did not handle message id %i\n", message.msgid);
break;
}
} // end: switch msgid
if(++nMsgHandled >= NUM_MSG_HANDLE)return;
}
}
void mavlink_handleMessage(mavlink_message_t* msg) {
mavlink_message_t msg2;
char sysmsg_str[1024];
switch (msg->msgid) {
case MAVLINK_MSG_ID_HEARTBEAT: {
mavlink_heartbeat_t packet;
mavlink_msg_heartbeat_decode(msg, &packet);
droneType = packet.type;
autoPilot = packet.autopilot;
if (packet.base_mode == MAV_MODE_MANUAL_ARMED) {
ModelData.mode = MODEL_MODE_MANUAL;
} else if (packet.base_mode == 128 + 64 + 16) {
ModelData.mode = MODEL_MODE_RTL;
} else if (packet.base_mode == 128 + 16) {
ModelData.mode = MODEL_MODE_POSHOLD;
} else if (packet.base_mode == 128 + 4) {
ModelData.mode = MODEL_MODE_MISSION;
}
if (packet.system_status == MAV_STATE_ACTIVE) {
ModelData.armed = MODEL_ARMED;
} else {
ModelData.armed = MODEL_DISARMED;
}
// SDL_Log("Heartbeat: %i, %i, %i\n", ModelData.armed, ModelData.mode, ModelData.status);
ModelData.heartbeat = 100;
// sprintf(sysmsg_str, "Heartbeat: %i", (int)time(0));
if ((*msg).sysid != 0xff) {
ModelData.sysid = (*msg).sysid;
ModelData.compid = (*msg).compid;
if (mavlink_maxparam == 0) {
mavlink_start_feeds();
}
}
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: {
mavlink_rc_channels_scaled_t packet;
mavlink_msg_rc_channels_scaled_decode(msg, &packet);
// SDL_Log("Radio: %i,%i,%i\n", packet.chan1_scaled, packet.chan2_scaled, packet.chan3_scaled);
/* if ((int)packet.chan6_scaled > 1000) {
mode = MODE_MISSION;
} else if ((int)packet.chan6_scaled < -1000) {
mode = MODE_MANUEL;
} else {
mode = MODE_POSHOLD;
}
if ((int)packet.chan7_scaled > 1000) {
mode = MODE_RTL;
} else if ((int)packet.chan7_scaled < -1000) {
mode = MODE_SETHOME;
}
*/
ModelData.radio[0] = (int)packet.chan1_scaled / 100.0;
ModelData.radio[1] = (int)packet.chan2_scaled / 100.0;
ModelData.radio[2] = (int)packet.chan3_scaled / 100.0;
ModelData.radio[3] = (int)packet.chan4_scaled / 100.0;
ModelData.radio[4] = (int)packet.chan5_scaled / 100.0;
ModelData.radio[5] = (int)packet.chan6_scaled / 100.0;
ModelData.radio[6] = (int)packet.chan7_scaled / 100.0;
ModelData.radio[7] = (int)packet.chan8_scaled / 100.0;
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_SCALED_PRESSURE: {
mavlink_scaled_pressure_t packet;
mavlink_msg_scaled_pressure_decode(msg, &packet);
// SDL_Log("BAR;%i;%0.2f;%0.2f;%0.2f\n", time(0), packet.press_abs, packet.press_diff, packet.temperature / 100.0);
// redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_ATTITUDE: {
mavlink_attitude_t packet;
mavlink_msg_attitude_decode(msg, &packet);
ModelData.roll = toDeg(packet.roll);
ModelData.pitch = toDeg(packet.pitch);
if (toDeg(packet.yaw) < 0.0) {
ModelData.yaw = 360.0 + toDeg(packet.yaw);
} else {
ModelData.yaw = toDeg(packet.yaw);
}
mavlink_update_yaw = 1;
// SDL_Log("ATT;%i;%0.2f;%0.2f;%0.2f\n", time(0), toDeg(packet.roll), toDeg(packet.pitch), toDeg(packet.yaw));
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_SCALED_IMU: {
// SDL_Log("SCALED_IMU\n");
break;
}
case MAVLINK_MSG_ID_GPS_RAW_INT: {
mavlink_gps_raw_int_t packet;
mavlink_msg_gps_raw_int_decode(msg, &packet);
if (packet.lat != 0.0) {
GPS_found = 1;
ModelData.p_lat = (float)packet.lat / 10000000.0;
ModelData.p_long = (float)packet.lon / 10000000.0;
ModelData.p_alt = (float)packet.alt / 1000.0;
ModelData.speed = (float)packet.vel / 100.0;
ModelData.numSat = packet.satellites_visible;
ModelData.gpsfix = packet.fix_type;
redraw_flag = 1;
}
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_RAW: {
// SDL_Log("RC_CHANNELS_RAW\n");
break;
}
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: {
// SDL_Log("SERVO_OUTPUT_RAW\n");
break;
}
case MAVLINK_MSG_ID_SYS_STATUS: {
mavlink_sys_status_t packet;
mavlink_msg_sys_status_decode(msg, &packet);
// SDL_Log("%0.1f %%, %0.3f V)\n", packet.load / 10.0, packet.voltage_battery / 1000.0);
ModelData.voltage = packet.voltage_battery / 1000.0;
ModelData.load = packet.load / 10.0;
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_STATUSTEXT: {
mavlink_statustext_t packet;
mavlink_msg_statustext_decode(msg, &packet);
SDL_Log("mavlink: ## %s ##\n", packet.text);
sys_message((char *)packet.text);
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_PARAM_VALUE: {
mavlink_param_value_t packet;
mavlink_msg_param_value_decode(msg, &packet);
char var[101];
uint16_t n1 = 0;
uint16_t n2 = 0;
for (n1 = 0; n1 < strlen(packet.param_id); n1++) {
if (packet.param_id[n1] != 9 && packet.param_id[n1] != ' ' && packet.param_id[n1] != '\t') {
var[n2++] = packet.param_id[n1];
}
}
var[n2++] = 0;
// MAV_VAR_FLOAT=0, /* 32 bit float | */
// MAV_VAR_UINT8=1, /* 8 bit unsigned integer | */
// MAV_VAR_INT8=2, /* 8 bit signed integer | */
// MAV_VAR_UINT16=3, /* 16 bit unsigned integer | */
// MAV_VAR_INT16=4, /* 16 bit signed integer | */
// MAV_VAR_UINT32=5, /* 32 bit unsigned integer | */
// MAV_VAR_INT32=6, /* 32 bit signed integer | */
sprintf(sysmsg_str, "PARAM_VALUE (%i/%i): #%s# = %f (Type: %i)", packet.param_index + 1, packet.param_count, var, packet.param_value, packet.param_type);
SDL_Log("mavlink: %s\n", sysmsg_str);
sys_message(sysmsg_str);
mavlink_maxparam = packet.param_count;
mavlink_timeout = 0;
mavlink_set_value(var, packet.param_value, packet.param_type, packet.param_index);
if (packet.param_index + 1 == packet.param_count || packet.param_index % 10 == 0) {
mavlink_param_xml_meta_load();
}
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_MISSION_COUNT: {
mavlink_mission_count_t packet;
mavlink_msg_mission_count_decode(msg, &packet);
sprintf(sysmsg_str, "MISSION_COUNT: %i\n", packet.count);
sys_message(sysmsg_str);
mission_max = packet.count;
if (mission_max > 0) {
mavlink_msg_mission_request_pack(127, 0, &msg2, ModelData.sysid, ModelData.compid, 0);
mavlink_send_message(&msg2);
}
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_MISSION_ACK: {
SDL_Log("mavlink: Mission-Transfer ACK\n");
break;
}
case MAVLINK_MSG_ID_MISSION_REQUEST: {
mavlink_mission_request_t packet;
mavlink_msg_mission_request_decode(msg, &packet);
uint16_t id = packet.seq;
uint16_t id2 = packet.seq;
uint16_t type = 0;
if (ModelData.teletype == TELETYPE_MEGAPIRATE_NG || ModelData.teletype == TELETYPE_ARDUPILOT) {
if (id2 > 0) {
id2 = id2 - 1;
} else {
SDL_Log("mavlink: WORKAROUND: first WP == HOME ?\n");
}
}
sprintf(sysmsg_str, "sending Waypoint (%i): %s\n", id, WayPoints[1 + id2].name);
sys_message(sysmsg_str);
if (strcmp(WayPoints[1 + id2].command, "WAYPOINT") == 0) {
SDL_Log("mavlink: Type: MAV_CMD_NAV_WAYPOINT\n");
type = MAV_CMD_NAV_WAYPOINT;
} else if (strcmp(WayPoints[1 + id2].command, "RTL") == 0) {
SDL_Log("mavlink: Type: MAV_CMD_NAV_RETURN_TO_LAUNCH\n");
type = MAV_CMD_NAV_RETURN_TO_LAUNCH;
} else if (strcmp(WayPoints[1 + id2].command, "LAND") == 0) {
SDL_Log("mavlink: Type: MAV_CMD_NAV_LAND\n");
type = MAV_CMD_NAV_LAND;
} else if (strcmp(WayPoints[1 + id2].command, "TAKEOFF") == 0) {
SDL_Log("mavlink: Type: MAV_CMD_NAV_TAKEOFF\n");
type = MAV_CMD_NAV_TAKEOFF;
} else {
SDL_Log("mavlink: Type: UNKNOWN\n");
type = MAV_CMD_NAV_WAYPOINT;
}
sprintf(sysmsg_str, "SENDING MISSION_ITEM: %i: %f, %f, %f\n", id, WayPoints[1 + id2].p_lat, WayPoints[1 + id2].p_long, WayPoints[1 + id2].p_alt);
SDL_Log("mavlink: %s\n", sysmsg_str);
mavlink_msg_mission_item_pack(127, 0, &msg2, ModelData.sysid, ModelData.compid, id, 0, type, 0.0, 0.0, WayPoints[1 + id2].radius, WayPoints[1 + id2].wait, WayPoints[1 + id2].orbit, WayPoints[1 + id2].yaw, WayPoints[1 + id2].p_lat, WayPoints[1 + id2].p_long, WayPoints[1 + id2].p_alt);
mavlink_send_message(&msg2);
/*
mavlink_msg_mission_item_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z );
float param1; ///< PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters
float param2; ///< PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds
float param3; ///< PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise.
float param4; ///< PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH
float x; ///< PARAM5 / local: x position, global: latitude
float y; ///< PARAM6 / y position: global: longitude
float z; ///< PARAM7 / z position: global: altitude
uint16_t seq; ///< Sequence
uint16_t command; ///< The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
uint8_t frame; ///< The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h
uint8_t current; ///< false:0, true:1
uint8_t autocontinue; ///< autocontinue to next wp
*/
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_MISSION_ITEM: {
mavlink_mission_item_t packet;
mavlink_msg_mission_item_decode(msg, &packet);
sprintf(sysmsg_str, "RECEIVED MISSION_ITEM: %i/%i: %f, %f, %f (%i)\n", packet.seq, mission_max, packet.x, packet.y, packet.z, packet.frame);
SDL_Log("mavlink: %s\n", sysmsg_str);
sys_message(sysmsg_str);
if (packet.seq < mission_max - 1) {
mavlink_msg_mission_request_pack(127, 0, &msg2, ModelData.sysid, ModelData.compid, packet.seq + 1);
mavlink_send_message(&msg2);
} else {
mavlink_msg_mission_ack_pack(127, 0, &msg2, ModelData.sysid, ModelData.compid, 15);
mavlink_send_message(&msg2);
}
if (ModelData.teletype == TELETYPE_MEGAPIRATE_NG || ModelData.teletype == TELETYPE_ARDUPILOT) {
if (packet.seq > 0) {
packet.seq = packet.seq - 1;
} else {
SDL_Log("mavlink: WORKAROUND: ignore first WP\n");
break;
}
}
SDL_Log("mavlink: getting WP(%i): %f, %f\n", packet.seq, packet.x, packet.y);
switch (packet.command) {
case MAV_CMD_NAV_WAYPOINT: {
strcpy(WayPoints[1 + packet.seq].command, "WAYPOINT");
break;
}
case MAV_CMD_NAV_LOITER_UNLIM: {
strcpy(WayPoints[1 + packet.seq].command, "LOITER_UNLIM");
break;
}
case MAV_CMD_NAV_LOITER_TURNS: {
strcpy(WayPoints[1 + packet.seq].command, "LOITER_TURNS");
break;
}
case MAV_CMD_NAV_LOITER_TIME: {
strcpy(WayPoints[1 + packet.seq].command, "LOITER_TIME");
break;
}
case MAV_CMD_NAV_RETURN_TO_LAUNCH: {
strcpy(WayPoints[1 + packet.seq].command, "RTL");
break;
}
case MAV_CMD_NAV_LAND: {
strcpy(WayPoints[1 + packet.seq].command, "LAND");
break;
}
case MAV_CMD_NAV_TAKEOFF: {
strcpy(WayPoints[1 + packet.seq].command, "TAKEOFF");
break;
}
default: {
sprintf(WayPoints[1 + packet.seq].command, "CMD:%i", packet.command);
break;
}
}
if (packet.x == 0.0) {
packet.x = 0.00001;
}
if (packet.y == 0.0) {
packet.y = 0.00001;
}
if (packet.z == 0.0) {
packet.z = 0.00001;
}
WayPoints[1 + packet.seq].p_lat = packet.x;
WayPoints[1 + packet.seq].p_long = packet.y;
WayPoints[1 + packet.seq].p_alt = packet.z;
WayPoints[1 + packet.seq].yaw = packet.param4;
sprintf(WayPoints[1 + packet.seq].name, "WP%i", packet.seq + 1);
WayPoints[1 + packet.seq + 1].p_lat = 0.0;
WayPoints[1 + packet.seq + 1].p_long = 0.0;
WayPoints[1 + packet.seq + 1].p_alt = 0.0;
WayPoints[1 + packet.seq + 1].yaw = 0.0;
WayPoints[1 + packet.seq + 1].name[0] = 0;
WayPoints[1 + packet.seq + 1].command[0] = 0;
/*
float param1; ///< PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters
float param2; ///< PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds
float param3; ///< PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise.
float param4; ///< PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH
float x; ///< PARAM5 / local: x position, global: latitude
float y; ///< PARAM6 / y position: global: longitude
float z; ///< PARAM7 / z position: global: altitude
uint16_t seq; ///< Sequence
uint16_t command; ///< The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
uint8_t frame; ///< The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h
uint8_t current; ///< false:0, true:1
uint8_t autocontinue; ///< autocontinue to next wp
GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="0" name="MAV_FRAME_GLOBAL">
GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="1" name="MAV_FRAME_LOCAL_NED">
GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="2" name="MAV_FRAME_MISSION">
GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="3" name="MAV_FRAME_GLOBAL_RELATIVE_ALT">
GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="4" name="MAV_FRAME_LOCAL_ENU">
*/
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_MISSION_CURRENT: {
mavlink_mission_current_t packet;
mavlink_msg_mission_current_decode(msg, &packet);
// SDL_Log("mavlink: ## Active_WP %f ##\n", packet.seq);
uav_active_waypoint = (uint8_t)packet.seq;
break;
}
case MAVLINK_MSG_ID_RAW_IMU: {
mavlink_raw_imu_t packet;
mavlink_msg_raw_imu_decode(msg, &packet);
/*
SDL_Log("## IMU_RAW_ACC_X %i ##\n", packet.xacc);
SDL_Log("## IMU_RAW_ACC_Y %i ##\n", packet.yacc);
SDL_Log("## IMU_RAW_ACC_Z %i ##\n", packet.zacc);
SDL_Log("## IMU_RAW_GYRO_X %i ##\n", packet.xgyro);
SDL_Log("## IMU_RAW_GYRO_Y %i ##\n", packet.ygyro);
SDL_Log("## IMU_RAW_GYRO_Z %i ##\n", packet.zgyro);
SDL_Log("## IMU_RAW_MAG_X %i ##\n", packet.xmag);
SDL_Log("## IMU_RAW_MAG_Y %i ##\n", packet.ymag);
SDL_Log("## IMU_RAW_MAG_Z %i ##\n", packet.zmag);
*/
ModelData.acc_x = (float)packet.xacc / 1000.0;
ModelData.acc_y = (float)packet.yacc / 1000.0;
ModelData.acc_z = (float)packet.zacc / 1000.0;
ModelData.gyro_x = (float)packet.zgyro;
ModelData.gyro_y = (float)packet.zgyro;
ModelData.gyro_z = (float)packet.zgyro;
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: {
mavlink_nav_controller_output_t packet;
mavlink_msg_nav_controller_output_decode(msg, &packet);
/*
nav_roll
nav_pitch
alt_error
aspd_error
xtrack_error
nav_bearing
target_bearing
wp_dist
*/
break;
}
case MAVLINK_MSG_ID_VFR_HUD: {
mavlink_vfr_hud_t packet;
mavlink_msg_vfr_hud_decode(msg, &packet);
// SDL_Log("## pa %f ##\n", packet.airspeed);
// SDL_Log("## pg %f ##\n", packet.groundspeed);
// SDL_Log("## palt %f ##\n", packet.alt);
if (GPS_found == 0) {
ModelData.p_alt = packet.alt;
}
// SDL_Log("## pc %f ##\n", packet.climb);
// SDL_Log("## ph %i ##\n", packet.heading);
// SDL_Log("## pt %i ##\n", packet.throttle);
break;
}
case MAVLINK_MSG_ID_RADIO: {
mavlink_radio_t packet;
mavlink_msg_radio_decode(msg, &packet);
SDL_Log("mavlink: ## rxerrors %i ##\n", packet.rxerrors);
SDL_Log("mavlink: ## fixed %i ##\n", packet.fixed);
SDL_Log("mavlink: ## rssi %i ##\n", packet.rssi);
SDL_Log("mavlink: ## remrssi %i ##\n", packet.remrssi);
SDL_Log("mavlink: ## txbuf %i ##\n", packet.txbuf);
SDL_Log("mavlink: ## noise %i ##\n", packet.noise);
SDL_Log("mavlink: ## remnoise %i ##\n", packet.remnoise);
break;
}
default: {
// SDL_Log(" ## MSG_ID == %i ##\n", msg->msgid);
break;
}
}
}
