2011-09-28 21:51:12 -03:00
|
|
|
/*
|
|
|
|
* AP_CommLink.cpp
|
|
|
|
*
|
|
|
|
* Created on: Apr 30, 2011
|
|
|
|
* Author: jgoppert
|
|
|
|
*/
|
|
|
|
|
2011-10-16 03:55:34 -03:00
|
|
|
#include "../FastSerial/FastSerial.h"
|
2011-09-28 21:51:12 -03:00
|
|
|
#include "AP_CommLink.h"
|
2011-10-16 03:55:34 -03:00
|
|
|
#include "AP_Navigator.h"
|
|
|
|
#include "AP_Guide.h"
|
|
|
|
#include "AP_Controller.h"
|
2011-10-26 15:59:40 -03:00
|
|
|
#include "AP_MavlinkCommand.h"
|
2011-10-16 03:55:34 -03:00
|
|
|
#include "AP_HardwareAbstractionLayer.h"
|
|
|
|
#include "AP_RcChannel.h"
|
|
|
|
#include "../AP_GPS/AP_GPS.h"
|
|
|
|
#include "../AP_Math/AP_Math.h"
|
|
|
|
#include "../AP_IMU/AP_IMU.h"
|
|
|
|
#include "../AP_Compass/AP_Compass.h"
|
|
|
|
#include "AP_BatteryMonitor.h"
|
2011-09-28 21:51:12 -03:00
|
|
|
|
|
|
|
namespace apo {
|
|
|
|
|
|
|
|
uint8_t MavlinkComm::_nChannels = 0;
|
|
|
|
uint8_t MavlinkComm::_paramNameLengthMax = 13;
|
|
|
|
|
2011-10-16 03:55:34 -03:00
|
|
|
AP_CommLink::AP_CommLink(FastSerial * link, AP_Navigator * navigator, AP_Guide * guide,
|
2011-10-26 13:31:11 -03:00
|
|
|
AP_Controller * controller, AP_HardwareAbstractionLayer * hal) :
|
|
|
|
_link(link), _navigator(navigator), _guide(guide),
|
|
|
|
_controller(controller), _hal(hal) {
|
2011-10-16 03:55:34 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
MavlinkComm::MavlinkComm(FastSerial * link, AP_Navigator * nav, AP_Guide * guide,
|
2011-10-26 13:31:11 -03:00
|
|
|
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;
|
|
|
|
}
|
2011-10-16 03:55:34 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void MavlinkComm::send() {
|
2011-10-26 13:31:11 -03:00
|
|
|
// if number of channels exceeded return
|
|
|
|
if (_channel == MAVLINK_COMM_3)
|
|
|
|
return;
|
2011-10-16 03:55:34 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void MavlinkComm::sendMessage(uint8_t id, uint32_t param) {
|
2011-10-26 13:31:11 -03:00
|
|
|
//_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;
|
|
|
|
}
|
|
|
|
|
2011-11-15 18:15:54 -04:00
|
|
|
case MAVLINK_MSG_ID_LOCAL_POSITION: {
|
|
|
|
mavlink_msg_local_position_send(_channel, timeStamp,
|
|
|
|
_navigator->getPN(),_navigator->getPE(), _navigator->getPD(),
|
|
|
|
_navigator->getVN(), _navigator->getVE(), _navigator->getVD());
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
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,
|
2011-11-15 18:15:54 -04:00
|
|
|
_navigator->getGroundSpeed()*1000.0,
|
2011-10-26 13:31:11 -03:00
|
|
|
_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: {
|
2011-11-19 22:39:14 -04:00
|
|
|
int16_t xmag, ymag, zmag;
|
|
|
|
xmag = ymag = zmag = 0;
|
|
|
|
if (_hal->compass) {
|
|
|
|
// XXX THIS IS NOT SCALED
|
|
|
|
xmag = _hal->compass->mag_x;
|
|
|
|
ymag = _hal->compass->mag_y;
|
|
|
|
zmag = _hal->compass->mag_z;
|
|
|
|
}
|
|
|
|
mavlink_msg_scaled_imu_send(_channel, timeStamp,
|
|
|
|
_navigator->getXAccel()*1e3,
|
|
|
|
_navigator->getYAccel()*1e3,
|
|
|
|
_navigator->getZAccel()*1e3,
|
|
|
|
_navigator->getRollRate()*1e3,
|
|
|
|
_navigator->getPitchRate()*1e3,
|
|
|
|
_navigator->getYawRate()*1e3,
|
|
|
|
xmag, ymag, zmag);
|
|
|
|
break;
|
2011-10-26 13:31:11 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
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: {
|
|
|
|
|
|
|
|
uint16_t batteryVoltage = 0; // (milli volts)
|
|
|
|
uint16_t batteryPercentage = 1000; // times 10
|
|
|
|
if (_hal->batteryMonitor) {
|
|
|
|
batteryPercentage = _hal->batteryMonitor->getPercentage()*10;
|
|
|
|
batteryVoltage = _hal->batteryMonitor->getVoltage()*1000;
|
|
|
|
}
|
|
|
|
mavlink_msg_sys_status_send(_channel, _controller->getMode(),
|
|
|
|
_guide->getMode(), _hal->getState(), _hal->load * 10,
|
|
|
|
batteryVoltage, batteryPercentage, _packetDrops);
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
case MAVLINK_MSG_ID_WAYPOINT_ACK: {
|
|
|
|
sendText(SEVERITY_LOW, PSTR("waypoint ack"));
|
|
|
|
//mavlink_waypoint_ack_t packet;
|
|
|
|
uint8_t type = 0; // ok (0), error(1)
|
|
|
|
mavlink_msg_waypoint_ack_send(_channel, _cmdDestSysId,
|
|
|
|
_cmdDestCompId, type);
|
|
|
|
|
|
|
|
// turn off waypoint send
|
|
|
|
_receivingCmds = false;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
case MAVLINK_MSG_ID_WAYPOINT_CURRENT: {
|
|
|
|
mavlink_msg_waypoint_current_send(_channel,
|
|
|
|
_guide->getCurrentIndex());
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
default: {
|
|
|
|
char msg[50];
|
|
|
|
sprintf(msg, "autopilot sending unknown command with id: %d", id);
|
|
|
|
sendText(SEVERITY_HIGH, msg);
|
|
|
|
}
|
|
|
|
|
|
|
|
} // switch
|
2011-10-16 03:55:34 -03:00
|
|
|
} // send message
|
|
|
|
|
|
|
|
void MavlinkComm::receive() {
|
2011-10-26 13:31:11 -03:00
|
|
|
//_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;
|
|
|
|
status.packet_rx_drop_count = 0;
|
|
|
|
|
|
|
|
// 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;
|
2011-10-16 03:55:34 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void MavlinkComm::sendText(uint8_t severity, const char *str) {
|
2011-10-26 13:31:11 -03:00
|
|
|
mavlink_msg_statustext_send(_channel, severity, (const int8_t*) str);
|
2011-10-16 03:55:34 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void MavlinkComm::sendText(uint8_t severity, const prog_char_t *str) {
|
2011-10-26 13:31:11 -03:00
|
|
|
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);
|
2011-10-16 03:55:34 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void MavlinkComm::acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2) {
|
|
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
|
|
* sends parameters one at a time
|
|
|
|
*/
|
|
|
|
void MavlinkComm::sendParameters() {
|
2011-10-26 13:31:11 -03:00
|
|
|
//_hal->debug->printf_P(PSTR("send parameters\n"));
|
|
|
|
// Check to see if we are sending parameters
|
|
|
|
while (NULL != _queuedParameter) {
|
|
|
|
AP_Var *vp;
|
|
|
|
float value;
|
2011-10-16 03:55:34 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
// copy the current parameter and prepare to move to the next
|
|
|
|
vp = _queuedParameter;
|
|
|
|
_queuedParameter = _queuedParameter->next();
|
2011-10-16 03:55:34 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
// 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)) {
|
2011-10-16 03:55:34 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
char paramName[_paramNameLengthMax];
|
|
|
|
vp->copy_name(paramName, sizeof(paramName));
|
2011-10-16 03:55:34 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
mavlink_msg_param_value_send(_channel, (int8_t*) paramName,
|
|
|
|
value, _countParameters(), _queuedParameterIndex);
|
2011-10-16 03:55:34 -03:00
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
_queuedParameterIndex++;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
2011-10-16 03:55:34 -03:00
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
|
|
* request commands one at a time
|
|
|
|
*/
|
|
|
|
void MavlinkComm::requestCmds() {
|
2011-10-26 13:31:11 -03:00
|
|
|
//_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);
|
|
|
|
}
|
2011-10-16 03:55:34 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
void MavlinkComm::_handleMessage(mavlink_message_t * msg) {
|
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
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;
|
|
|
|
}
|
|
|
|
|
2011-11-15 18:15:54 -04:00
|
|
|
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);
|
2011-11-19 22:39:14 -04:00
|
|
|
_navigator->setXAccel(packet.xacc/ 1e3);
|
|
|
|
_navigator->setYAccel(packet.xacc/ 1e3);
|
|
|
|
_navigator->setZAccel(packet.xacc/ 1e3);
|
2011-11-15 18:15:54 -04:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
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;
|
|
|
|
|
2011-11-19 22:39:14 -04:00
|
|
|
case MAV_ACTION_MOTORS_START:
|
|
|
|
_hal->setState(MAV_STATE_ACTIVE);
|
|
|
|
break;
|
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
case MAV_ACTION_CALIBRATE_GYRO:
|
|
|
|
case MAV_ACTION_CALIBRATE_MAG:
|
|
|
|
case MAV_ACTION_CALIBRATE_ACC:
|
|
|
|
case MAV_ACTION_CALIBRATE_PRESSURE:
|
2011-11-19 22:39:14 -04:00
|
|
|
_hal->setState(MAV_STATE_STANDBY);
|
|
|
|
_navigator->calibrate();
|
|
|
|
break;
|
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
case MAV_ACTION_EMCY_KILL:
|
2011-11-19 22:39:14 -04:00
|
|
|
case MAV_ACTION_CONFIRM_KILL:
|
2011-10-26 13:31:11 -03:00
|
|
|
case MAV_ACTION_MOTORS_STOP:
|
|
|
|
case MAV_ACTION_SHUTDOWN:
|
2011-11-19 22:39:14 -04:00
|
|
|
_hal->setState(MAV_STATE_STANDBY);
|
|
|
|
_controller->setMode(MAV_MODE_LOCKED);
|
|
|
|
break;
|
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
case MAV_ACTION_LAUNCH:
|
2011-11-19 22:39:14 -04:00
|
|
|
case MAV_ACTION_TAKEOFF:
|
|
|
|
_controller->setMode(MAV_MODE_AUTO);
|
|
|
|
_guide->setMode(MAV_NAV_LIFTOFF);
|
|
|
|
break;
|
|
|
|
|
|
|
|
case MAV_ACTION_LAND:
|
|
|
|
_guide->setCurrentIndex(0);
|
|
|
|
_guide->setMode(MAV_NAV_LANDING);
|
|
|
|
break;
|
|
|
|
|
2011-10-26 13:31:11 -03:00
|
|
|
case MAV_ACTION_EMCY_LAND:
|
2011-11-19 22:39:14 -04:00
|
|
|
_guide->setMode(MAV_NAV_LANDING);
|
|
|
|
break;
|
|
|
|
|
|
|
|
case MAV_ACTION_LOITER:
|
2011-10-26 13:31:11 -03:00
|
|
|
case MAV_ACTION_HALT:
|
2011-11-19 22:39:14 -04:00
|
|
|
_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:
|
2011-10-26 13:31:11 -03:00
|
|
|
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 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
|
|
|
|
|
|
|
|
|
|
|
|
}
|
2011-10-16 03:55:34 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
uint16_t MavlinkComm::_countParameters() {
|
2011-10-26 13:31:11 -03:00
|
|
|
// 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;
|
2011-10-16 03:55:34 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
AP_Var * _findParameter(uint16_t index) {
|
2011-10-26 13:31:11 -03:00
|
|
|
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;
|
2011-10-16 03:55:34 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// check the target
|
|
|
|
uint8_t MavlinkComm::_checkTarget(uint8_t sysid, uint8_t compid) {
|
2011-10-26 13:31:11 -03:00
|
|
|
/*
|
|
|
|
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
|
|
|
|
}
|
2011-10-16 03:55:34 -03:00
|
|
|
}
|
|
|
|
|
2011-09-28 21:51:12 -03:00
|
|
|
} // apo
|
2011-10-26 15:59:40 -03:00
|
|
|
// vim:ts=4:sw=4:expandtab
|