Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/APO/AP_CommLink.h

776 lines
20 KiB
C++
Raw Blame History

/*
* AP_CommLink.h
* Copyright (C) James Goppert 2010 <james.goppert@gmail.com>
*
* 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 <http://www.gnu.org/licenses/>.
*/
#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),
// 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_NB_HIGH;
break;
}
}
virtual void send() {
// if number of channels exceeded return
if (_channel == MAVLINK_COMM_NB_HIGH)
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_NB_HIGH)
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 (int 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 (int 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, prev_cell, temp;
temp = analogRead(0);
batteryVoltage = ((temp*5/1023)/0.28);
mavlink_msg_sys_status_send(_channel, _controller->getMode(),
_guide->getMode(), _hal->state(), _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_NB_HIGH)
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 <= _guide->getNumberOfCommands()) {
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;
Vector<mavlink_command_t *> _cmdList;
// parameters
static uint8_t _paramNameLengthMax;
uint16_t _parameterCount;
AP_Var * _queuedParameter;
uint16_t _queuedParameterIndex;
uint16_t _cmdMax;
// 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->setHeading(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);
mavlink_waypoint_t msg = cmd.convert(_guide->getCurrentIndex());
mavlink_msg_waypoint_send(_channel, _cmdDestSysId, _cmdDestCompId,
msg.seq, msg.frame, msg.command, msg.current, msg.autocontinue,
msg.param1, msg.param2, msg.param3, msg.param4, msg.x, msg.y,
msg.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);
if (_checkTarget(packet.target_system, packet.target_component))
break;
// set current waypoint
_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;
}
_guide->setNumberOfCommands(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 msg[50];
sprintf(msg,
"waypoint request out of sequence: (packet) %d / %d (ap)",
packet.seq, _cmdRequestIndex);
sendText(SEVERITY_HIGH, msg);
*/
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 >= _guide->getNumberOfCommands()) {
sendMessage( MAVLINK_MSG_ID_WAYPOINT_ACK);
//sendText(SEVERITY_LOW, PSTR("waypoint ack sent"));
}
_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
Reference in New Issue View Git Blame Copy Permalink