mirror of https://github.com/ArduPilot/ardupilot
791 lines
21 KiB
C++
791 lines
21 KiB
C++
/*
|
|
* 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), _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<mavlink_command_t *> _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
|