/* * AP_CommLink.h * Copyright (C) James Goppert 2010 * * This file is free software: you can redistribute it and/or modify it * under the terms of the GNU General Public License as published by the * Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This file is distributed in the hope that it will be useful, but * WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. * See the GNU General Public License for more details. * * You should have received a copy of the GNU General Public License along * with this program. If not, see . */ #ifndef AP_CommLink_H #define AP_CommLink_H #include "AP_HardwareAbstractionLayer.h" #include "AP_MavlinkCommand.h" #include "AP_Controller.h" namespace apo { class AP_Controller; class AP_Navigator; class AP_Guide; class AP_HardwareAbstractionLayer; enum { SEVERITY_LOW, SEVERITY_MED, SEVERITY_HIGH }; // forward declarations //class ArduPilotOne; //class AP_Controller; /// CommLink class class AP_CommLink { public: AP_CommLink(FastSerial * link, AP_Navigator * navigator, AP_Guide * guide, AP_Controller * controller, AP_HardwareAbstractionLayer * hal) : _link(link), _navigator(navigator), _guide(guide), _controller(controller), _hal(hal) { } virtual void send() = 0; virtual void receive() = 0; virtual void sendMessage(uint8_t id, uint32_t param = 0) = 0; virtual void sendText(uint8_t severity, const char *str) = 0; virtual void sendText(uint8_t severity, const prog_char_t *str) = 0; virtual void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) = 0; virtual void sendParameters() = 0; virtual void requestCmds() = 0; protected: FastSerial * _link; AP_Navigator * _navigator; AP_Guide * _guide; AP_Controller * _controller; AP_HardwareAbstractionLayer * _hal; }; class MavlinkComm: public AP_CommLink { public: MavlinkComm(FastSerial * link, AP_Navigator * nav, AP_Guide * guide, AP_Controller * controller, AP_HardwareAbstractionLayer * hal) : AP_CommLink(link, nav, guide, controller, hal), // options _useRelativeAlt(true), // commands _sendingCmds(false), _receivingCmds(false), _cmdTimeLastSent(millis()), _cmdTimeLastReceived(millis()), _cmdDestSysId(0), _cmdDestCompId(0), _cmdRequestIndex(0), _cmdMax(30), _cmdNumberRequested(0), // parameters _parameterCount(0), _queuedParameter(NULL), _queuedParameterIndex(0) { switch (_nChannels) { case 0: mavlink_comm_0_port = link; _channel = MAVLINK_COMM_0; _nChannels++; break; case 1: mavlink_comm_1_port = link; _channel = MAVLINK_COMM_1; _nChannels++; break; default: // signal that number of channels exceeded _channel = MAVLINK_COMM_3; break; } } virtual void send() { // if number of channels exceeded return if (_channel == MAVLINK_COMM_3) return; } void sendMessage(uint8_t id, uint32_t param = 0) { //_hal->debug->printf_P(PSTR("send message\n")); // if number of channels exceeded return if (_channel == MAVLINK_COMM_3) return; uint64_t timeStamp = micros(); switch (id) { case MAVLINK_MSG_ID_HEARTBEAT: { mavlink_msg_heartbeat_send(_channel, mavlink_system.type, MAV_AUTOPILOT_ARDUPILOTMEGA); break; } case MAVLINK_MSG_ID_ATTITUDE: { mavlink_msg_attitude_send(_channel, timeStamp, _navigator->getRoll(), _navigator->getPitch(), _navigator->getYaw(), _navigator->getRollRate(), _navigator->getPitchRate(), _navigator->getYawRate()); break; } case MAVLINK_MSG_ID_GLOBAL_POSITION: { mavlink_msg_global_position_send(_channel, timeStamp, _navigator->getLat() * rad2Deg, _navigator->getLon() * rad2Deg, _navigator->getAlt(), _navigator->getVN(), _navigator->getVE(), _navigator->getVD()); break; } case MAVLINK_MSG_ID_GPS_RAW: { mavlink_msg_gps_raw_send(_channel, timeStamp, _hal->gps->status(), _navigator->getLat() * rad2Deg, _navigator->getLon() * rad2Deg, _navigator->getAlt(), 0, 0, _navigator->getGroundSpeed(), _navigator->getYaw() * rad2Deg); break; } /* case MAVLINK_MSG_ID_GPS_RAW_INT: { mavlink_msg_gps_raw_int_send(_channel,timeStamp,_hal->gps->status(), _navigator->getLat_degInt(), _navigator->getLon_degInt(),_navigator->getAlt_intM(), 0,0, _navigator->getGroundSpeed(),_navigator->getYaw()*rad2Deg); break; } */ case MAVLINK_MSG_ID_SCALED_IMU: { /* * accel/gyro debug */ /* Vector3f accel = _hal->imu->get_accel(); Vector3f gyro = _hal->imu->get_gyro(); Serial.printf_P(PSTR("accel: %f %f %f gyro: %f %f %f\n"), accel.x,accel.y,accel.z,gyro.x,gyro.y,gyro.z); */ Vector3f accel = _hal->imu->get_accel(); Vector3f gyro = _hal->imu->get_gyro(); mavlink_msg_raw_imu_send(_channel, timeStamp, 1000 * accel.x, 1000 * accel.y, 1000 * accel.z, 1000 * gyro.x, 1000 * gyro.y, 1000 * gyro.z, _hal->compass->mag_x, _hal->compass->mag_y, _hal->compass->mag_z); // XXX THIS IS NOT SCALED FOR MAG } case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: { int16_t ch[8]; for (int i = 0; i < 8; i++) ch[i] = 0; for (uint8_t i = 0; i < 8 && i < _hal->rc.getSize(); i++) { ch[i] = 10000 * _hal->rc[i]->getPosition(); //_hal->debug->printf_P(PSTR("ch: %d position: %d\n"),i,ch[i]); } mavlink_msg_rc_channels_scaled_send(_channel, ch[0], ch[1], ch[2], ch[3], ch[4], ch[5], ch[6], ch[7], 255); break; } case MAVLINK_MSG_ID_RC_CHANNELS_RAW: { int16_t ch[8]; for (int i = 0; i < 8; i++) ch[i] = 0; for (uint8_t i = 0; i < 8 && i < _hal->rc.getSize(); i++) ch[i] = _hal->rc[i]->getPwm(); mavlink_msg_rc_channels_raw_send(_channel, ch[0], ch[1], ch[2], ch[3], ch[4], ch[5], ch[6], ch[7], 255); break; } case MAVLINK_MSG_ID_SYS_STATUS: { float batteryVoltage, temp; temp = analogRead(0); batteryVoltage = ((temp * 5 / 1023) / 0.28); mavlink_msg_sys_status_send(_channel, _controller->getMode(), _guide->getMode(), _hal->getState(), _hal->load * 10, batteryVoltage * 1000, (batteryVoltage - 3.3) / (4.2 - 3.3) * 1000, _packetDrops); break; } case MAVLINK_MSG_ID_WAYPOINT_ACK: { sendText(SEVERITY_LOW, PSTR("waypoint ack")); //mavlink_waypoint_ack_t packet; uint8_t type = 0; // ok (0), error(1) mavlink_msg_waypoint_ack_send(_channel, _cmdDestSysId, _cmdDestCompId, type); // turn off waypoint send _receivingCmds = false; break; } case MAVLINK_MSG_ID_WAYPOINT_CURRENT: { mavlink_msg_waypoint_current_send(_channel, _guide->getCurrentIndex()); break; } default: { char msg[50]; sprintf(msg, "autopilot sending unknown command with id: %d", id); sendText(SEVERITY_HIGH, msg); } } // switch } // send message virtual void receive() { //_hal->debug->printf_P(PSTR("receive\n")); // if number of channels exceeded return // if (_channel == MAVLINK_COMM_3) return; // receive new packets mavlink_message_t msg; mavlink_status_t status; // process received bytes while (comm_get_available(_channel)) { uint8_t c = comm_receive_ch(_channel); // Try to get a new message if (mavlink_parse_char(_channel, c, &msg, &status)) _handleMessage(&msg); } // Update packet drops counter _packetDrops += status.packet_rx_drop_count; } void sendText(uint8_t severity, const char *str) { mavlink_msg_statustext_send(_channel, severity, (const int8_t*) str); } void sendText(uint8_t severity, const prog_char_t *str) { mavlink_statustext_t m; uint8_t i; for (i = 0; i < sizeof(m.text); i++) { m.text[i] = pgm_read_byte((const prog_char *) (str++)); } if (i < sizeof(m.text)) m.text[i] = 0; sendText(severity, (const char *) m.text); } void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) { } /** * sends parameters one at a time */ void sendParameters() { //_hal->debug->printf_P(PSTR("send parameters\n")); // Check to see if we are sending parameters while (NULL != _queuedParameter) { AP_Var *vp; float value; // copy the current parameter and prepare to move to the next vp = _queuedParameter; _queuedParameter = _queuedParameter->next(); // if the parameter can be cast to float, report it here and break out of the loop value = vp->cast_to_float(); if (!isnan(value)) { char paramName[_paramNameLengthMax]; vp->copy_name(paramName, sizeof(paramName)); mavlink_msg_param_value_send(_channel, (int8_t*) paramName, value, _countParameters(), _queuedParameterIndex); _queuedParameterIndex++; break; } } } /** * request commands one at a time */ void requestCmds() { //_hal->debug->printf_P(PSTR("requesting commands\n")); // request cmds one by one if (_receivingCmds && _cmdRequestIndex <= _cmdNumberRequested) { mavlink_msg_waypoint_request_send(_channel, _cmdDestSysId, _cmdDestCompId, _cmdRequestIndex); } } private: // options bool _useRelativeAlt; // commands bool _sendingCmds; bool _receivingCmds; uint16_t _cmdTimeLastSent; uint16_t _cmdTimeLastReceived; uint16_t _cmdDestSysId; uint16_t _cmdDestCompId; uint16_t _cmdRequestIndex; uint16_t _cmdNumberRequested; uint16_t _cmdMax; Vector _cmdList; // parameters static uint8_t _paramNameLengthMax; uint16_t _parameterCount; AP_Var * _queuedParameter; uint16_t _queuedParameterIndex; // channel mavlink_channel_t _channel; uint16_t _packetDrops; static uint8_t _nChannels; void _handleMessage(mavlink_message_t * msg) { uint32_t timeStamp = micros(); switch (msg->msgid) { //_hal->debug->printf_P(PSTR("message received: %d"), msg->msgid); case MAVLINK_MSG_ID_HEARTBEAT: { mavlink_heartbeat_t packet; mavlink_msg_heartbeat_decode(msg, &packet); _hal->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); //_hal->debug->printf_P(PSTR("received hil gps raw packet\n")); /* _hal->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_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); //_hal->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_CALIBRATE_RC: case MAV_ACTION_CALIBRATE_GYRO: case MAV_ACTION_CALIBRATE_MAG: case MAV_ACTION_CALIBRATE_ACC: case MAV_ACTION_CALIBRATE_PRESSURE: case MAV_ACTION_REBOOT: case MAV_ACTION_REC_START: case MAV_ACTION_REC_PAUSE: case MAV_ACTION_REC_STOP: case MAV_ACTION_TAKEOFF: case MAV_ACTION_LAND: case MAV_ACTION_NAVIGATE: case MAV_ACTION_LOITER: case MAV_ACTION_MOTORS_START: case MAV_ACTION_CONFIRM_KILL: case MAV_ACTION_EMCY_KILL: case MAV_ACTION_MOTORS_STOP: case MAV_ACTION_SHUTDOWN: case MAV_ACTION_CONTINUE: case MAV_ACTION_SET_MANUAL: case MAV_ACTION_SET_AUTO: case MAV_ACTION_LAUNCH: case MAV_ACTION_RETURN: case MAV_ACTION_EMCY_LAND: case MAV_ACTION_HALT: 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; _hal->debug->printf_P(PSTR("sequence: %d\n"),packet.seq); AP_MavlinkCommand cmd(packet.seq); cmd.load(); 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; } _hal->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); //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 } } uint16_t _countParameters() { // if we haven't cached the parameter count yet... if (0 == _parameterCount) { AP_Var *vp; vp = AP_Var::first(); do { // if a parameter responds to cast_to_float then we are going to be able to report it if (!isnan(vp->cast_to_float())) { _parameterCount++; } } while (NULL != (vp = vp->next())); } return _parameterCount; } AP_Var * _findParameter(uint16_t index) { AP_Var *vp; vp = AP_Var::first(); while (NULL != vp) { // if the parameter is reportable if (!(isnan(vp->cast_to_float()))) { // if we have counted down to the index we want if (0 == index) { // return the parameter return vp; } // count off this parameter, as it is reportable but not // the one we want index--; } // and move to the next parameter vp = vp->next(); } return NULL; } // check the target uint8_t _checkTarget(uint8_t sysid, uint8_t compid) { /* char msg[50]; sprintf(msg, "target = %d / %d\tcomp = %d / %d", sysid, mavlink_system.sysid, compid, mavlink_system.compid); sendText(SEVERITY_LOW, msg); */ if (sysid != mavlink_system.sysid) { //sendText(SEVERITY_LOW, PSTR("system id mismatch")); return 1; } else if (compid != mavlink_system.compid) { //sendText(SEVERITY_LOW, PSTR("component id mismatch")); return 0; // XXX currently not receiving correct compid from gcs } else { return 0; // no error } } }; } // namespace apo #endif // AP_CommLink_H // vim:ts=4:sw=4:tw=78:expandtab