void ofxModbusTcpClient::writeMultipleCoils(int _id, int _startAddress, vector<bool> _newValues) {
if (enabled) {
if (_id<=numberOfSlaves && _id>0 && _startAddress>0 && _startAddress<=2000) {
uint8_t localByteArray[6+(_newValues.size()*2)];
int totB = ceil((float)_newValues.size()/(float)8); //Always in multiples of 8
WORD length = 4 + (totB);
WORD start = _startAddress-1;
localByteArray[0] = HIGHBYTE(0); //Transaction High
localByteArray[1] = LOWBYTE(0); //Transaction Low
localByteArray[2] = 0x00; //Protocal Identifier High
localByteArray[3] = 0x00; //Protocol Identifier Low
localByteArray[4] = HIGHBYTE(length); //Length High
localByteArray[5] = LOWBYTE(length); //Length Low
localByteArray[6] = _id; //Unit Idenfifier
localByteArray[7] = 0x0f; //Function Code
localByteArray[8] = HIGHBYTE(start); //Start Address High
localByteArray[9] = LOWBYTE(start); //Start Address Low
//New Value
int cb = 10; //Starting Write Byte
int cv = 0; //Starting bit
for (int p=0; p<_newValues.size(); p++) {
if (_newValues.at(p)) { //Set Bit
localByteArray[cb] |= 1 << cv;
} else { //Clear Bit
localByteArray[cb] &= ~(1 << cv);
}
cv++; //Move to Next Bit
if (cv==8) {
cv = 0; //Reset Bits
cb++; //Move to next Byte
}
}
stringstream dm;
dm<<"Writing Multiple Coils to "<<_id<<" Start Address:"<<_startAddress<<" Qty:"<<_newValues.size();
vector<uint8_t> lba;
int sizeOfArray = sizeof(localByteArray) / sizeof(localByteArray[0]);
for (int i=0; i<sizeOfArray; i++){
lba.push_back(localByteArray[i]);
}
mbCommand c;
c.msg = lba;
c.length = length;
c.timeAdded = ofGetElapsedTimeMillis();
c.debugString = dm.str();
commandToSend.push_back(c);
} else {
ofLogError("ofxModbusTCP IP:"+ip)<<"Write Multiple Coil Parameters Are Out Of Range";
}
}
}
//Slave Writes
void ofxModbusTcpClient::writeCoil(int _id, int _startAddress, bool _newValue) {
if (enabled) {
if (_id<=numberOfSlaves && _id>0 && _startAddress>0 && _startAddress<=2000) {
uint8_t localByteArray[12];
WORD length = 6;
WORD start = _startAddress-1;
localByteArray[0] = HIGHBYTE(0); //Transaction High
localByteArray[1] = LOWBYTE(0); //Transaction Low
localByteArray[2] = 0x00; //Protocal Identifier High
localByteArray[3] = 0x00; //Protocol Identifier Low
localByteArray[4] = HIGHBYTE(length); //Length High
localByteArray[5] = LOWBYTE(length); //Length Low
localByteArray[6] = _id; //Unit Idenfifier
localByteArray[7] = 0x05; //Function Code
localByteArray[8] = HIGHBYTE(start); //Start Address High
localByteArray[9] = LOWBYTE(start); //Start Address Low
//New Value
if (_newValue) {
localByteArray[10] = 0xff; //New Value High
localByteArray[11] = 0x00; //New Value Low
} else {
localByteArray[10] = 0x00; //New Value High
localByteArray[11] = 0x00; //New Value Low
}
stringstream dm;
dm<<"Writing Coil of "<<_id<<" Address:"<<_startAddress<<" Value:"<<_newValue;
vector<uint8_t> lba;
int sizeOfArray = sizeof(localByteArray) / sizeof(localByteArray[0]);
for (int i=0; i<sizeOfArray; i++){
lba.push_back(localByteArray[i]);
}
mbCommand c;
c.msg = lba;
c.length = length;
c.timeAdded = ofGetElapsedTimeMillis();
c.debugString = dm.str();
commandToSend.push_back(c);
} else {
ofLogError("ofxModbusTCP IP:"+ip)<<"Write Coil Parameters Are Out Of Range";
}
}
}
TIDorb::types::BigInt TIDorb::types::BigInt::operator- (const TIDorb::types::BigInt &bi) const
{
TIDorb::types::BigInt res;
if ((*this) < bi) {
res = bi - (*this);
res.sign *= -1;
return res;
}
// apartir de Aqui (*this) siempre mayor que bi.
if (sign == bi.sign) {
if(sign == 0) {
//ambos son 0.
res = 0;
return res;
}
int j;
unsigned int ireg = 0;
unsigned int borrow = 0;
bool isCero = true;
for (j=0; j<MAG_SIZE; j++) {
ireg=(256+mag[j]) - bi[j] - borrow;
if(ireg>=256)
borrow=0;
else borrow=1;
res[j]=LOWBYTE(ireg);
if(res[j]!=0)
isCero=false;
}
if(isCero) {
res.sign=0;
}
else res.sign=1;
}
else if (sign == -1) {
res = -(*this) + bi;
res.sign = -1;
}
else if (bi.sign == -1) {
res = (*this) + (-bi);
}
else if (sign == 0) {
res = -bi;
}
else if (bi.sign == 0) {
res = (*this);
}
return res;
}
/*!
\brief handle USB device class request
\param[in] pudev: pointer to USB device instance
\param[in] req: pointer to USB device class request
\param[out] none
\retval USB device operation status
*/
static usbd_status_enum usbd_device_class_request (usb_core_handle_struct *pudev, usb_device_req_struct *req)
{
usbd_status_enum ret = USBD_OK;
switch (pudev->dev.status) {
case USB_STATUS_CONFIGURED:
if (LOWBYTE(req->wIndex) <= USBD_ITF_MAX_NUM) {
ret = (usbd_status_enum)(pudev->dev.class_req_handler(pudev, req));
if ((0U == req->wLength) && (USBD_OK == ret)) {
/* no data stage */
usbd_ctlstatus_tx(pudev);
}
} else {
usbd_enum_error(pudev, req);
}
break;
default:
usbd_enum_error(pudev, req);
break;
}
return ret;
}
void ofxModbusTcpClient::sendNextCommand(){
if (commandToSend.size()>0){
waitingForReply = true;
//Set last function code
lastFunctionCode = commandToSend[0].msg[7];
//set transaction id
WORD tx = getTransactionID();
commandToSend[0].msg[0] = HIGHBYTE(tx); //Transaction High
commandToSend[0].msg[1] = LOWBYTE(tx); //Transaction Low
//load vector to local uint8_t
uint8_t localByteArray[commandToSend[0].msg.size()];
for (int i=0; i<commandToSend[0].msg.size(); i++){
localByteArray[i] = commandToSend[0].msg[i];
}
//send command
unsigned char * t = localByteArray;
tcpClient.sendRawBytes((const char*)t, 6+commandToSend[0].length);
//Log it
sendDebug("Sent: "+commandToSend[0].debugString);
//erase last command
commandToSend.erase(commandToSend.begin());
}
}
// verify_circle - check if we have circled the point enough
bool Copter::verify_circle(const AP_Mission::Mission_Command& cmd)
{
// check if we've reached the edge
if (auto_mode == Auto_CircleMoveToEdge) {
if (wp_nav.reached_wp_destination()) {
Vector3f curr_pos = inertial_nav.get_position();
Vector3f circle_center = pv_location_to_vector(cmd.content.location);
// set target altitude if not provided
if (is_zero(circle_center.z)) {
circle_center.z = curr_pos.z;
}
// set lat/lon position if not provided
if (cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
circle_center.x = curr_pos.x;
circle_center.y = curr_pos.y;
}
// start circling
auto_circle_start();
}
return false;
}
// check if we have completed circling
return fabsf(circle_nav.get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1);
}
void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
{
//set target alt
next_WP_loc.alt = cmd.content.location.alt;
// convert relative alt to absolute alt
if (cmd.content.location.flags.relative_alt) {
next_WP_loc.flags.relative_alt = false;
next_WP_loc.alt += home.alt;
}
//I know I'm storing this twice -- I'm doing that on purpose --
//see verify_loiter_alt() function
condition_value = next_WP_loc.alt;
//are lat and lon 0? if so, don't change the current wp lat/lon
if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) {
set_next_WP(cmd.content.location);
}
//set loiter direction
loiter_set_direction_wp(cmd);
//must the plane be heading towards the next waypoint before breaking?
condition_value2 = LOWBYTE(cmd.p1);
}
void ofxModbusTcpClient::writeMultipleRegisters(int _id, int _startAddress, vector<int> _newValues) {
if (enabled) {
if (_id<=numberOfSlaves && _id>0 && _startAddress>0 && _startAddress<=123) {
uint8_t localByteArray[6+(_newValues.size()*2)];
WORD length = 4 + (_newValues.size() * 2);
WORD start = _startAddress-1;
localByteArray[0] = HIGHBYTE(0); //Transaction High
localByteArray[1] = LOWBYTE(0); //Transaction Low
localByteArray[2] = 0x00; //Protocal Identifier High
localByteArray[3] = 0x00; //Protocol Identifier Low
localByteArray[4] = HIGHBYTE(length); //Length High
localByteArray[5] = LOWBYTE(length); //Length Low
localByteArray[6] = _id; //Unit Idenfifier
localByteArray[7] = 0x10; //Function Code
localByteArray[8] = HIGHBYTE(start); //Start Address High
localByteArray[9] = LOWBYTE(start); //Start Address Low
//New Value
int ct = 10; //Starting Bit
for (int i=0; i<_newValues.size(); i++) {
localByteArray[ct] = HIGHBYTE(_newValues.at(i)); //New Value High
localByteArray[ct+1] = LOWBYTE(_newValues.at(i)); //New Value Low
ct = ct+2; //Increase to next two bits
}
stringstream dm;
dm<<"Writing Multiple Registers to "<<_id<<" Start Address:"<<_startAddress<<" Qty:"<<_newValues.size();
vector<uint8_t> lba;
int sizeOfArray = sizeof(localByteArray) / sizeof(localByteArray[0]);
for (int i=0; i<sizeOfArray; i++){
lba.push_back(localByteArray[i]);
}
mbCommand c;
c.msg = lba;
c.length = length;
c.timeAdded = ofGetElapsedTimeMillis();
c.debugString = dm.str();
commandToSend.push_back(c);
} else {
ofLogError("ofxModbusTCP IP:"+ip)<<"Write Multiple Register Parameters Are Out Of Range";
}
}
}
// send message to sensor
void AP_RangeFinder_BLPing::send_message(uint16_t msgid, const uint8_t *payload, uint16_t payload_len)
{
if (uart == nullptr) {
return;
}
// check for sufficient space in outgoing buffer
if (uart->txspace() < payload_len + 10U) {
return;
}
// write header
uart->write((uint8_t)BLPING_FRAME_HEADER1);
uart->write((uint8_t)BLPING_FRAME_HEADER2);
uint16_t crc = BLPING_FRAME_HEADER1 + BLPING_FRAME_HEADER2;
// write payload length
uart->write(LOWBYTE(payload_len));
uart->write(HIGHBYTE(payload_len));
crc += LOWBYTE(payload_len) + HIGHBYTE(payload_len);
// msgid
uart->write(LOWBYTE(msgid));
uart->write(HIGHBYTE(msgid));
crc += LOWBYTE(msgid) + HIGHBYTE(msgid);
// src dev id
uart->write((uint8_t)BLPING_SRC_ID);
crc += BLPING_SRC_ID;
// destination dev id
uart->write((uint8_t)BLPING_DEST_ID);
crc += BLPING_DEST_ID;
// payload
if (payload != nullptr) {
for (uint16_t i = 0; i<payload_len; i++) {
uart->write(payload[i]);
crc += payload[i];
}
}
// checksum
uart->write(LOWBYTE(crc));
uart->write(HIGHBYTE(crc));
}
void ofxModbusTcpClient::updateRegisters(int _id, int _startAddress, int _qty) {
if (enabled) {
if (_id<=numberOfSlaves && _id>0 && _qty<=125 && _qty>0) {
uint8_t localByteArray[12];
WORD tx = getTransactionID();
WORD length = 6;
WORD start = _startAddress-1;
WORD qty = _qty;
localByteArray[0] = HIGHBYTE(tx); //Transaction High
localByteArray[1] = LOWBYTE(tx); //Transaction Low
localByteArray[2] = 0x00; //Protocal Identifier High
localByteArray[3] = 0x00; //Protocol Identifier Low
localByteArray[4] = HIGHBYTE(length); //Length High
localByteArray[5] = LOWBYTE(length); //Length Low
localByteArray[6] = _id; //Unit Idenfifier
localByteArray[7] = 0x03; //Function Code
localByteArray[8] = HIGHBYTE(start); //Start Address High
localByteArray[9] = LOWBYTE(start); //Start Address Low
localByteArray[10] = HIGHBYTE(qty); //Qty High
localByteArray[11] = LOWBYTE(qty); //Qty Low
lastStartingReg = start;
stringstream dm;
dm<<"Reading Registers of "<<_id<<" Start:"<<_startAddress<<" Qty:"<<_qty;
vector<uint8_t> lba;
int sizeOfArray = sizeof(localByteArray) / sizeof(localByteArray[0]);
for (int i=0; i<sizeOfArray; i++){
lba.push_back(localByteArray[i]);
}
mbCommand c;
c.msg = lba;
c.length = length;
c.timeAdded = ofGetElapsedTimeMillis();
c.debugString = dm.str();
commandToSend.push_back(c);
} else {
ofLogError("ofxModbusTCP IP:"+ip)<<"Read Registers Parameters Are Out Of Range";
}
}
}
void NUMstring_add (unsigned char *a, unsigned char *b, unsigned char *c, long n) {
int j;
unsigned short reg = 0;
for (j = n; j > 1; j--) {
reg = a[j] + b[j] + HIGHBYTE (reg);
c[j + 1] = LOWBYTE (reg);
}
}
void Plane::do_loiter_turns(const AP_Mission::Mission_Command& cmd)
{
Location cmdloc = cmd.content.location;
location_sanitize(current_loc, cmdloc);
set_next_WP(cmdloc);
loiter_set_direction_wp(cmd);
loiter.total_cd = (uint32_t)(LOWBYTE(cmd.p1)) * 36000UL;
condition_value = 1; // used to signify primary turns goal not yet met
}
void hidecursor(boolean curs) /* Hides cursor from the screen */
{
unsigned short offset;
CursorHidden = curs;
if (curs == TRUE)
{
offset = scrheight * scrwidth; /* This really only moves the cursor one line off the screen */
outportb(crtc_mem + 0, 14); /* MSB of offset to CRTC reg 14 */
outportb(crtc_mem + 1, HIGHBYTE(offset));
outportb(crtc_mem + 0, 15); /* LSB of offset to CRTC reg 15 */
outportb(crtc_mem + 1, LOWBYTE(offset));
}
}
void tty_setcursor(int wno, unsigned x, unsigned y)
{
/*unsigned short crtc_adr = 0x3D4; 0x3B4 for monochrome */
unsigned short offset;
if ((window_info[wno].movcursor == TRUE) && (CursorHidden == FALSE))
{
offset = (x + window_info[wno].xhome) + (y + window_info[wno].yhome) * scrwidth; /* 80 characters per line */
outportb(crtc_mem , 14); /* MSB of offset to CRTC reg 14 */
outportb(crtc_mem + 1, HIGHBYTE(offset));
outportb(crtc_mem , 15); /* LSB of offset to CRTC reg 15 */
outportb(crtc_mem + 1, LOWBYTE(offset));
}
}
/*!
*******************************************************************************
**
** \brief Closes an I2C channel.
**
** \param handleP Pointer to handle which can be used to control the hardware.
**
** \return Possible return codes:
** - \c #GD_OK
** - \c #GD_ERR_NOT_INITIALIZED
**
** \sa GD_I2C_Open()
**
******************************************************************************/
GERR GD_I2C_Close(GD_HANDLE *handleP)
{
GD_I2C_SPEED_E speed;
speed = (GD_I2C_SPEED_E)LOWBYTE(*handleP);
if ((speed != GD_I2C_100KBPS &&
speed != GD_I2C_400KBPS )
)
return GD_ERR_BAD_PARAMETER;
*handleP = NULL;
return GD_OK;
}
TIDorb::types::BigInt TIDorb::types::BigInt::operator+ (const TIDorb::types::BigInt& bi) const
{
TIDorb::types::BigInt res;
if (sign == 0) { // Direct assign
res = bi;
return res;
}
else if (bi.sign == 0) {
res = (*this);
return res;
}
if (sign == bi.sign) {
unsigned int resInt = 0;
for (int i=0; i<MAG_SIZE; i++) {
resInt = mag[i] + bi[i] + HIBYTE(resInt);
res[i] = LOWBYTE(resInt);
}
if (HIBYTE(resInt) != 0) {
throw CORBA::DATA_CONVERSION("BigInt::operator+: HIBYTE(restInt) is not 0");
}
res.sign = sign;
return res;
}
else if (sign == -1) {
// Add negative to positive: (+X) + (-Y)
// is the same that: (+X) - (+Y)
res = bi - (-(*this));
return res;
}
else if(sign == 1) {
// Added a positive to negavie: (+X) + (-Y)
// is the same that: (+X) - (+Y)
res = (*this) - bi;
return *this;
}
return res;
}
/*
* Assign operator for integer values
*/
TIDorb::types::BigInt& TIDorb::types::BigInt::operator= (int val)
{
if (val > 0) {
sign =+ 1;
} else if (val < 0) {
val *=-1; // Take care with this. If not it will be in 2 or 1 complement
sign =-1;
}
else sign = 0;
for (int i = 0; i < MAG_SIZE; i++)
mag[i] = 0;
// the operation is:
mag[0] = LOWBYTE(val);
mag[1] = HIBYTE(val);
_modified = true;
return *this;
}
/*
* Constructor from integer
* Initilizes "mag" to zero and assigns the integer to index 0 and 1 at "mag"
*/
TIDorb::types::BigInt::BigInt(int val)
{
if (val > 0) {
sign = 1;
} else if ( val < 0 ) {
sign = -1;
val *= -1;
}
else
sign = 0;
for(int i = 0; i < MAG_SIZE; i++)
mag[i] = 0;
// Init mag with integer value:
mag[0] = LOWBYTE(val);
mag[1] = HIBYTE(val);
_str = NULL;
_hex_str = NULL;
_modified = true;
}
TIDorb::types::BigInt TIDorb::types::BigInt::operator*(int v) const
{
TIDorb::types::BigInt res;
if ( (sign==1) && (v!=0) ) {
unsigned int resInt = 0;
for (int i=0; i<MAG_SIZE; i++) {
resInt = mag[i]*v + HIBYTE(resInt);
res[i] = LOWBYTE(resInt);
}
if (HIBYTE(resInt) != 0) {
throw CORBA::DATA_CONVERSION("BigInt::operator*: HIBYTE(resInt) is not 0");
}
int new_sign = (sign * v);
if (new_sign > 0) {
res.sign = 1;
}
else if (new_sign < 0) {
res.sign = -1;
}
else
res.sign = 0;
}
else if ( (sign==0) || (v==0) ) {
res = 0;
}
else if (sign == -1) {
res = ( (-(*this)) * (-v));
}
return res;
}
void TIDorb::types::BigInt::shiftLeft(int pos)
{
if (pos == 0)
return;
unsigned int currentIntMag = 0;
unsigned char previousMag = 0;
for (int i=0; i < MAG_SIZE; i++) {
unsigned char value = mag[i];
currentIntMag = value;
currentIntMag = currentIntMag << pos;
mag[i] = LOWBYTE(currentIntMag) + previousMag;
previousMag = HIBYTE(currentIntMag);
}
if (previousMag > 0) {
throw CORBA::DATA_CONVERSION("BigInt::shiftLeft: previousMag > 0");
}
_modified = true;
}
void pca9685_led_write( uint8_t i2c_addr, uint8_t led, uint16_t value ) {
// Toggle second LED on i2c write
PORTB ^= 0x02;
i2c_start( i2c_addr + PCA9685_WRITE );
i2c_write( PCA9685_LED0 + 4*led );
uint16_t phase = (PWM_MAX-value)-1;
i2c_write( 0x00 );
i2c_write( 0x00 );
// Write LED ON
// i2c_write( (uint8_t) (phase & 0xFF ) );
// i2c_write( (uint8_t) (phase >> 8 ) );
// Write LED Off
i2c_write( LOWBYTE( value ) );
i2c_write( HIGHBYTE( value ) );
i2c_stop();
}
void Plane::do_loiter_turns(const AP_Mission::Mission_Command& cmd)
{
set_next_WP(cmd.content.location);
loiter.total_cd = (uint32_t)(LOWBYTE(cmd.p1)) * 36000UL;
loiter_set_direction_wp(cmd);
}
int CCommonFnc::APDU_ConvertToString(CARDAPDU* pAPDU, string_type* pString, BOOL toSendAPDU) {
int status = STAT_OK;
string_type message;
string_type ioData;
if (toSendAPDU) {
// APDU to SmartCard
CCommonFnc::BYTE_ConvertFromArrayToHexString(pAPDU->DataIn, pAPDU->lc, &ioData);
// FORMAT: INS CLA P1 P2 LC input_data Le
//message.Format("-> %.2x %.2x %.2x %.2x %.2x %s %.2x", pAPDU->cla, pAPDU->ins, pAPDU->p1, pAPDU->p2, pAPDU->lc, (LPCTSTR) ioData, pAPDU->le);
message = string_format(_CONV("-> %.2x %.2x %.2x %.2x %.2x %s %.2x"), pAPDU->cla, pAPDU->ins, pAPDU->p1, pAPDU->p2, pAPDU->lc, (LPCTSTR)ioData.c_str(), pAPDU->le);
}
else {
// APDU from SmartCard
CCommonFnc::BYTE_ConvertFromArrayToHexString(pAPDU->DataOut, pAPDU->le, &ioData);
// FORMAT: output_data SW
//message.Format("<- %s %.2x %.2x", (LPCTSTR) ioData, HIGHBYTE(pAPDU->sw), LOWBYTE(pAPDU->sw));
message = string_format(_CONV("<- %s %.2x %.2x"), (LPCTSTR)ioData.c_str(), HIGHBYTE(pAPDU->sw), LOWBYTE(pAPDU->sw));
}
*pString = message;
return status;
}
// mission_cmd_to_mavlink - converts an AP_Mission::Mission_Command object to a mavlink message which can be sent to the GCS
// return true on success, false on failure
bool AP_Mission::mission_cmd_to_mavlink(const AP_Mission::Mission_Command& cmd, mavlink_mission_item_t& packet)
{
bool copy_location = false;
bool copy_alt = false;
// command's position in mission list and mavlink id
packet.seq = cmd.index;
packet.command = cmd.id;
// set defaults
packet.current = 0; // 1 if we are passing back the mission command that is currently being executed
packet.param1 = 0;
packet.param2 = 0;
packet.param3 = 0;
packet.param4 = 0;
packet.autocontinue = 1;
// command specific conversions from mission command to mavlink packet
switch (cmd.id) {
case MAV_CMD_NAV_WAYPOINT: // MAV ID: 16
copy_location = true;
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
// acceptance radius in meters
packet.param2 = cmd.p1;
#else
// delay at waypoint in seconds
packet.param1 = cmd.p1;
#endif
break;
case MAV_CMD_NAV_LOITER_UNLIM: // MAV ID: 17
copy_location = true;
if (cmd.content.location.flags.loiter_ccw) {
packet.param3 = -1;
}else{
packet.param3 = 1;
}
break;
case MAV_CMD_NAV_LOITER_TURNS: // MAV ID: 18
copy_location = true;
packet.param1 = LOWBYTE(cmd.p1); // number of times to circle is held in low byte of p1
packet.param3 = HIGHBYTE(cmd.p1); // radius is held in high byte of p1
if (cmd.content.location.flags.loiter_ccw) {
packet.param3 = -packet.param3;
}
break;
case MAV_CMD_NAV_LOITER_TIME: // MAV ID: 19
copy_location = true;
packet.param1 = cmd.p1; // loiter time in seconds
if (cmd.content.location.flags.loiter_ccw) {
packet.param3 = -1;
}else{
packet.param3 = 1;
}
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH: // MAV ID: 20
copy_location = true;
break;
case MAV_CMD_NAV_LAND: // MAV ID: 21
copy_location = true;
break;
case MAV_CMD_NAV_TAKEOFF: // MAV ID: 22
copy_location = true; // only altitude is used
packet.param1 = cmd.p1; // minimum pitch (plane only)
break;
case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT: // MAV ID: 30
copy_location = true; //only using alt.
break;
case MAV_CMD_NAV_SPLINE_WAYPOINT: // MAV ID: 82
copy_location = true;
packet.param1 = cmd.p1; // delay at waypoint in seconds
break;
case MAV_CMD_NAV_GUIDED_ENABLE: // MAV ID: 92
packet.param1 = cmd.p1; // on/off. >0.5 means "on", hand-over control to external controller
break;
case MAV_CMD_CONDITION_DELAY: // MAV ID: 112
packet.param1 = cmd.content.delay.seconds; // delay in seconds
break;
case MAV_CMD_CONDITION_CHANGE_ALT: // MAV ID: 113
copy_alt = true; // only altitude is used
packet.param1 = cmd.content.location.lat / 100.0f; // climb/descent rate converted from cm/s to m/s. To-Do: store in proper climb_rate structure
break;
case MAV_CMD_CONDITION_DISTANCE: // MAV ID: 114
packet.param1 = cmd.content.distance.meters; // distance in meters from next waypoint
break;
case MAV_CMD_CONDITION_YAW: // MAV ID: 115
packet.param1 = cmd.content.yaw.angle_deg; // target angle in degrees
packet.param2 = cmd.content.yaw.turn_rate_dps; // 0 = use default turn rate otherwise specific turn rate in deg/sec
packet.param3 = cmd.content.yaw.direction; // -1 = ccw, +1 = cw
packet.param4 = cmd.content.yaw.relative_angle; // 0 = absolute angle provided, 1 = relative angle provided
break;
case MAV_CMD_DO_SET_MODE: // MAV ID: 176
packet.param1 = cmd.p1; // set flight mode identifier
break;
case MAV_CMD_DO_JUMP: // MAV ID: 177
packet.param1 = cmd.content.jump.target; // jump-to command number
packet.param2 = cmd.content.jump.num_times; // repeat count
break;
case MAV_CMD_DO_CHANGE_SPEED: // MAV ID: 178
packet.param1 = cmd.content.speed.speed_type; // 0 = airspeed, 1 = ground speed
packet.param2 = cmd.content.speed.target_ms; // speed in m/s
packet.param3 = cmd.content.speed.throttle_pct; // throttle as a percentage from 0 ~ 100%
break;
case MAV_CMD_DO_SET_HOME: // MAV ID: 179
copy_location = true;
packet.param1 = cmd.p1; // p1=0 means use current location, p=1 means use provided location
break;
case MAV_CMD_DO_SET_PARAMETER: // MAV ID: 180
packet.param1 = cmd.p1; // parameter number
packet.param2 = cmd.content.location.alt; // parameter value
break;
case MAV_CMD_DO_SET_RELAY: // MAV ID: 181
packet.param1 = cmd.content.relay.num; // relay number
packet.param2 = cmd.content.relay.state; // 0:off, 1:on
break;
case MAV_CMD_DO_REPEAT_RELAY: // MAV ID: 182
packet.param1 = cmd.content.repeat_relay.num; // relay number
packet.param2 = cmd.content.repeat_relay.repeat_count; // count
packet.param3 = cmd.content.repeat_relay.cycle_time; // time in seconds
break;
case MAV_CMD_DO_SET_SERVO: // MAV ID: 183
packet.param1 = cmd.content.servo.channel; // channel
packet.param2 = cmd.content.servo.pwm; // PWM
break;
case MAV_CMD_DO_REPEAT_SERVO: // MAV ID: 184
packet.param1 = cmd.content.repeat_servo.channel; // channel
packet.param2 = cmd.content.repeat_servo.pwm; // PWM
packet.param3 = cmd.content.repeat_servo.repeat_count; // count
packet.param4 = cmd.content.repeat_servo.cycle_time; // time in milliseconds converted to seconds
break;
case MAV_CMD_DO_LAND_START: // MAV ID: 189
copy_location = true;
break;
case MAV_CMD_DO_SET_ROI: // MAV ID: 201
copy_location = true;
packet.param1 = cmd.p1; // 0 = no roi, 1 = next waypoint, 2 = waypoint number, 3 = fixed location, 4 = given target (not supported)
break;
case MAV_CMD_DO_DIGICAM_CONTROL: // MAV ID: 203
// these commands takes no parameters
break;
case MAV_CMD_DO_MOUNT_CONTROL: // MAV ID: 205
packet.param1 = cmd.content.mount_control.pitch;
packet.param2 = cmd.content.mount_control.roll;
packet.param3 = cmd.content.mount_control.yaw;
break;
case MAV_CMD_DO_SET_CAM_TRIGG_DIST: // MAV ID: 206
packet.param1 = cmd.content.cam_trigg_dist.meters; // distance between camera shots in meters
break;
case MAV_CMD_DO_FENCE_ENABLE: // MAV ID: 207
packet.param1 = cmd.p1; // action 0=disable, 1=enable
break;
case MAV_CMD_DO_PARACHUTE: // MAV ID: 208
packet.param1 = cmd.p1; // action 0=disable, 1=enable, 2=release. See PARACHUTE_ACTION enum
break;
case MAV_CMD_DO_INVERTED_FLIGHT: // MAV ID: 210
packet.param1 = cmd.p1; // normal=0 inverted=1
break;
case MAV_CMD_DO_GRIPPER: // MAV ID: 211
packet.param1 = cmd.content.gripper.num; // gripper number
packet.param2 = cmd.content.gripper.action; // action 0=release, 1=grab. See GRIPPER_ACTION enum
break;
case MAV_CMD_DO_GUIDED_LIMITS: // MAV ID: 222
packet.param1 = cmd.p1; // max time in seconds the external controller will be allowed to control the vehicle
packet.param2 = cmd.content.guided_limits.alt_min; // min alt below which the command will be aborted. 0 for no lower alt limit
packet.param3 = cmd.content.guided_limits.alt_max; // max alt above which the command will be aborted. 0 for no upper alt limit
packet.param4 = cmd.content.guided_limits.horiz_max;// max horizontal distance the vehicle can move before the command will be aborted. 0 for no horizontal limit
break;
default:
// unrecognised command
return false;
}
// copy location from mavlink to command
if (copy_location) {
packet.x = cmd.content.location.lat / 1.0e7f; // int32_t latitude to float
packet.y = cmd.content.location.lng / 1.0e7f; // int32_t longitude to float
}
if (copy_location || copy_alt) {
packet.z = cmd.content.location.alt / 100.0f; // cmd alt in cm to m
if (cmd.content.location.flags.relative_alt) {
packet.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
}else{
packet.frame = MAV_FRAME_GLOBAL;
}
#if AP_TERRAIN_AVAILABLE
if (cmd.content.location.flags.terrain_alt) {
// this is a above-terrain altitude
if (!cmd.content.location.flags.relative_alt) {
// refuse to return non-relative terrain mission
// items. Internally we do have these, and they
// have home.alt added, but we should never be
// returning them to the GCS, as the GCS doesn't know
// our home.alt, so it would have no way to properly
// interpret it
return false;
}
packet.z = cmd.content.location.alt * 0.01f;
packet.frame = MAV_FRAME_GLOBAL_TERRAIN_ALT;
}
#else
// don't ever return terrain mission items if no terrain support
if (cmd.content.location.flags.terrain_alt) {
return false;
}
#endif
}
// if we got this far then it must have been successful
return true;
}
NTSTATUS
KeI386FlatToGdtSelector(
IN ULONG SelectorBase,
IN USHORT Length,
IN USHORT Selector
)
/*++
Routine Description:
This function converts a 32-bit flat address to a GDT selector-offset
pair. The segment set up is always 16-bit ring 0 data segment.
Arguments:
SelectorBase - Supplies 32 bit flat address to be set as the base address
of the desired selector.
Length - Supplies the Length of the segment. The Length is a 16 bit value
and zero means 64KB.
Selector - Supplies the selector to be set up.
Return Value:
STATUS_SUCCESS - If the requested LID is released.
STATUS_ABIOS_NOT_PRESENT - If there is no ABIOS support in the system.
STATUS_ABIOS_INVALID_SELECTOR - If the selector supplied is invalid.
--*/
{
PKGDTENTRY GdtEntry, GdtEntry1;
KIRQL OldIrql;
ULONG i;
if (!KiAbiosPresent) {
return STATUS_ABIOS_NOT_PRESENT;
}
if (Selector < RESERVED_GDT_ENTRIES * sizeof(KGDTENTRY)) {
return STATUS_ABIOS_INVALID_SELECTOR;
} else {
ExAcquireSpinLock(&KiAbiosGdtLock, &OldIrql);
GdtEntry = (PKGDTENTRY)(KiAbiosGdt[0] + Selector);
GdtEntry->LimitLow = (USHORT)(Length - 1);
GdtEntry->BaseLow = LOWWORD(SelectorBase);
GdtEntry->HighWord.Bytes.BaseMid = LOWBYTE(HIGHWORD(SelectorBase));
GdtEntry->HighWord.Bytes.BaseHi = HIGHBYTE(HIGHWORD(SelectorBase));
GdtEntry->HighWord.Bits.Pres = 1;
GdtEntry->HighWord.Bits.Type = TYPE_DATA;
GdtEntry->HighWord.Bits.Dpl = DPL_SYSTEM;
for (i = 1; i < (ULONG)KeNumberProcessors; i++) {
GdtEntry1 = (PKGDTENTRY)(KiAbiosGdt[i] + Selector);
*GdtEntry1 = *GdtEntry;
}
ExReleaseSpinLock(&KiAbiosGdtLock, OldIrql);
return STATUS_SUCCESS;
}
}
PERCFG |= BV(6); //set T1CFG to location alt. 2
P2SEL &= ~(BV(4)); //set PRI1P1 to timer1 priority
P2SEL |= BV(3); //set PRI0P1 to timer1 priority
T1CTL |= BV(1); //Enable modulo mode
T1CCTL1 |= BV(2); //Enable compare mode
T1CCTL1 |= (BV(5) | BV(4)); //Set compare mode 110: high when equal T1CC0, low when equal T1CC1
T1CCTL2 |= BV(2); //Enable compare mode
T1CCTL2 |= (BV(5) | BV(4)); //Set compare mode 110: high when equal T1CC0, low when equal T1CC2
T1CCTL3 |= BV(2); //Enable compare mode
T1CCTL3 |= (BV(5) | BV(4)); //Set compare mode 110: high when equal T1CC0, low when equal T1CC3
T1CCTL4 |= BV(2); //Enable compare mode
T1CCTL4 |= (BV(5) | BV(4)); //Set compare mode 110: high when equal T1CC0, low when equal T1CC4
T1CC0L = LOWBYTE(counter_top); //Counter top
T1CC0H = HIGHBYTE(counter_top);
T1CC1L = 0; //Counter compare for channel 1
T1CC1H = 0;
T1CC2L = 0; //Counter compare for channel 2
T1CC2H = 0;
T1CC3L = 0; //Counter compare for channel 3
T1CC3H = 0;
T1CC4L = 0; //Counter compare for channel 4
T1CC4H = 0;
}
void timer4_init() {
T4CTL |= (BV(7)|BV(6)|BV(5)); //Prescaler 128
T4CTL |= BV(2); //Clear
int CPCSCMngr::ExchangeAPDU(CARDAPDU* pAPDU) {
int status = STAT_OK;
if (m_cardContext && m_hCard) {
// CLEAR SOFTWARE RETURN STATUS
pAPDU->sw = SW_NO_ERROR;
// SEND APDU
//if ((status = TransmitAPDU(pAPDU, dataInLenForced)) == STAT_OK) {
if ((status = TransmitAPDUByCase(pAPDU)) == STAT_OK) {
// OK
// CHECK FOR SOFTWARE ERROR
if (pAPDU->sw == SW_NO_ERROR) {
// NO SOFWARE ERROR
}
else {
// CHECK FOR 'RESPONSE DATA AVAILABLE' STATUS
if ((pAPDU->sw & 0xFF00) == SW_BYTES_REMAINING_00) {
if (pAPDU->lc == 0x00) { // SYSTEM CALL TYPE (PROBABLY TO OBTAINING RESPONCE OUTPUT DATA)
// INSUFFICIENT OUTPUT DATA LENGTH
BYTE realLen = LOWBYTE(pAPDU->sw);
pAPDU->le = realLen;
// NEW APDU WITH REQUIRED OUTPUT DATA LENGTH
status = ExchangeAPDU(pAPDU);
}
else {
// USER CALL TYPE, OBTAIN RESPONSE DATA
BYTE realLen = LOWBYTE(pAPDU->sw);
CARDAPDU apdu;
// PREPARE SYSTEM APDU FOR RECIEVE RESPONSE OUTPUT DATA
apdu.cla = 0x00;
apdu.ins = 0xC0;
apdu.p1 = 0x00;
apdu.p2 = 0x00;
apdu.lc = 0x00;
apdu.le = realLen;
if ((status = ExchangeAPDU(&apdu)) == STAT_OK) {
// COPY RECIEVED APDU
memcpy(pAPDU->DataOut, apdu.DataOut, apdu.le);
pAPDU->le = apdu.le;
}
}
}
else {
// CHECK FOR 'CORRECT DATA LENGTH' STATUS
if (((pAPDU->sw & 0xFF00) == SW_CORRECT_LENGTH_00)){
pAPDU->le = LOWBYTE(pAPDU->sw);
}
else {
// SOFTWARE ERROR OCCURED
status = TranslateISO7816Error(pAPDU->sw);
}
}
}
}
}
else status = STAT_SESSION_NOT_OPEN;
return status;
}
// mission_cmd_to_mavlink - converts an AP_Mission::Mission_Command object to a mavlink message which can be sent to the GCS
// return true on success, false on failure
bool AP_Mission::mission_cmd_to_mavlink(const AP_Mission::Mission_Command& cmd, mavlink_mission_item_t& packet)
{
bool copy_location = false;
bool copy_alt = false;
// command's position in mission list and mavlink id
packet.seq = cmd.index;
packet.command = cmd.id;
// set defaults
packet.current = 0; // 1 if we are passing back the mission command that is currently being executed
packet.param1 = 0;
packet.param2 = 0;
packet.param3 = 0;
packet.param4 = 0;
packet.autocontinue = 1;
// command specific conversions from mission command to mavlink packet
switch (cmd.id) {
case MAV_CMD_NAV_WAYPOINT: // MAV ID: 16
copy_location = true;
packet.param1 = cmd.p1; // delay at waypoint in seconds
break;
case MAV_CMD_NAV_LOITER_UNLIM: // MAV ID: 17
copy_location = true;
if (cmd.content.location.flags.loiter_ccw) {
packet.param3 = -1;
}else{
packet.param3 = 1;
}
break;
case MAV_CMD_NAV_LOITER_TURNS: // MAV ID: 18
copy_location = true;
packet.param1 = LOWBYTE(cmd.p1); // number of times to circle is held in low byte of p1
packet.param3 = HIGHBYTE(cmd.p1); // radius is held in high byte of p1
if (cmd.content.location.flags.loiter_ccw) {
packet.param3 = -packet.param3;
}
break;
case MAV_CMD_NAV_LOITER_TIME: // MAV ID: 19
copy_location = true;
packet.param1 = cmd.p1; // loiter time in seconds
if (cmd.content.location.flags.loiter_ccw) {
packet.param3 = -1;
}else{
packet.param3 = 1;
}
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH: // MAV ID: 20
copy_location = true;
break;
case MAV_CMD_NAV_LAND: // MAV ID: 21
copy_location = true;
break;
case MAV_CMD_NAV_TAKEOFF: // MAV ID: 22
copy_location = true; // only altitude is used
packet.param1 = cmd.p1; // minimum pitch (plane only)
break;
case MAV_CMD_NAV_SPLINE_WAYPOINT: // MAV ID: 82
copy_location = true;
packet.param1 = cmd.p1; // delay at waypoint in seconds
break;
#ifdef MAV_CMD_NAV_GUIDED
case MAV_CMD_NAV_GUIDED: // MAV ID: 90
packet.param1 = cmd.p1; // max time in seconds the external controller will be allowed to control the vehicle
packet.param2 = cmd.content.nav_guided.alt_min; // min alt below which the command will be aborted. 0 for no lower alt limit
packet.param3 = cmd.content.nav_guided.alt_max; // max alt above which the command will be aborted. 0 for no upper alt limit
packet.param4 = cmd.content.nav_guided.horiz_max; // max horizontal distance the vehicle can move before the command will be aborted. 0 for no horizontal limit
break;
#endif
#ifdef MAV_CMD_NAV_VELOCITY
case MAV_CMD_NAV_VELOCITY: // MAV ID: 91
packet.param1 = cmd.p1; // frame - unused
packet.x = cmd.content.nav_velocity.x; // lat (i.e. north) velocity in m/s
packet.y = cmd.content.nav_velocity.y; // lon (i.e. east) velocity in m/s
packet.z = cmd.content.nav_velocity.z; // vertical (i.e. up) velocity in m/s
break;
#endif
case MAV_CMD_CONDITION_DELAY: // MAV ID: 112
packet.param1 = cmd.content.delay.seconds; // delay in seconds
break;
case MAV_CMD_CONDITION_CHANGE_ALT: // MAV ID: 113
copy_alt = true; // only altitude is used
packet.param1 = cmd.content.location.lat / 100.0f; // climb/descent rate converted from cm/s to m/s. To-Do: store in proper climb_rate structure
break;
case MAV_CMD_CONDITION_DISTANCE: // MAV ID: 114
packet.param1 = cmd.content.distance.meters; // distance in meters from next waypoint
break;
case MAV_CMD_CONDITION_YAW: // MAV ID: 115
packet.param1 = cmd.content.yaw.angle_deg; // target angle in degrees
packet.param2 = cmd.content.yaw.turn_rate_dps; // 0 = use default turn rate otherwise specific turn rate in deg/sec
packet.param3 = cmd.content.yaw.direction; // -1 = ccw, +1 = cw
packet.param4 = cmd.content.yaw.relative_angle; // 0 = absolute angle provided, 1 = relative angle provided
break;
case MAV_CMD_DO_SET_MODE: // MAV ID: 176
packet.param1 = cmd.p1; // set flight mode identifier
break;
case MAV_CMD_DO_JUMP: // MAV ID: 177
packet.param1 = cmd.content.jump.target; // jump-to command number
packet.param2 = cmd.content.jump.num_times; // repeat count
break;
case MAV_CMD_DO_CHANGE_SPEED: // MAV ID: 178
packet.param1 = cmd.content.speed.speed_type; // 0 = airspeed, 1 = ground speed
packet.param2 = cmd.content.speed.target_ms; // speed in m/s
packet.param3 = cmd.content.speed.throttle_pct; // throttle as a percentage from 0 ~ 100%
break;
case MAV_CMD_DO_SET_HOME: // MAV ID: 179
copy_location = true;
packet.param1 = cmd.p1; // p1=0 means use current location, p=1 means use provided location
break;
case MAV_CMD_DO_SET_PARAMETER: // MAV ID: 180
packet.param1 = cmd.p1; // parameter number
packet.param2 = cmd.content.location.alt; // parameter value
break;
case MAV_CMD_DO_SET_RELAY: // MAV ID: 181
packet.param1 = cmd.content.relay.num; // relay number
packet.param2 = cmd.content.relay.state; // 0:off, 1:on
break;
case MAV_CMD_DO_REPEAT_RELAY: // MAV ID: 182
packet.param1 = cmd.content.repeat_relay.num; // relay number
packet.param2 = cmd.content.repeat_relay.repeat_count; // count
packet.param3 = cmd.content.repeat_relay.cycle_time; // time in seconds
break;
case MAV_CMD_DO_SET_SERVO: // MAV ID: 183
packet.param1 = cmd.content.servo.channel; // channel
packet.param2 = cmd.content.servo.pwm; // PWM
break;
case MAV_CMD_DO_REPEAT_SERVO: // MAV ID: 184
packet.param1 = cmd.content.repeat_servo.channel; // channel
packet.param2 = cmd.content.repeat_servo.pwm; // PWM
packet.param3 = cmd.content.repeat_servo.repeat_count; // count
packet.param4 = cmd.content.repeat_servo.cycle_time; // time in milliseconds converted to seconds
break;
case MAV_CMD_DO_SET_ROI: // MAV ID: 201
copy_location = true;
packet.param1 = cmd.p1; // 0 = no roi, 1 = next waypoint, 2 = waypoint number, 3 = fixed location, 4 = given target (not supported)
break;
case MAV_CMD_DO_DIGICAM_CONTROL: // MAV ID: 203
case MAV_CMD_DO_MOUNT_CONTROL: // MAV ID: 205
// these commands takes no parameters
break;
case MAV_CMD_DO_SET_CAM_TRIGG_DIST: // MAV ID: 206
packet.param1 = cmd.content.cam_trigg_dist.meters; // distance between camera shots in meters
break;
case MAV_CMD_DO_PARACHUTE: // MAV ID: 208
packet.param1 = cmd.p1; // action 0=disable, 1=enable, 2=release. See PARACHUTE_ACTION enum
break;
case MAV_CMD_DO_INVERTED_FLIGHT: // MAV ID: 210
packet.param1 = cmd.p1; // normal=0 inverted=1
break;
default:
// unrecognised command
return false;
break;
}
// copy location from mavlink to command
if (copy_location) {
packet.x = cmd.content.location.lat / 1.0e7f; // int32_t latitude to float
packet.y = cmd.content.location.lng / 1.0e7f; // int32_t longitude to float
}
if (copy_location || copy_alt) {
packet.z = cmd.content.location.alt / 100.0f; // cmd alt in cm to m
if (cmd.content.location.flags.relative_alt) {
packet.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
}else{
packet.frame = MAV_FRAME_GLOBAL;
}
}
// if we got this far then it must have been successful
return true;
}
int tatl22_01Tx2G5_InternalPowerMeasure_Get(struct dth_element *elem, void *value)
{
int vl_Error = TAT_ERROR_OFF;
switch (elem->user_data) {
case ACT_TX2G5_INTPWRMEASURE_BAND:
v_tatrf_intpwrmeas_gsm_band = SetU16InU32(MSB_POSS,
v_tatrf_intpwrmeas_gsm_band,
v_tatrf_intpwrmeas_gsm_req_seq.band_MSB);
v_tatrf_intpwrmeas_gsm_band = SetU16InU32(LSB_POSS,
v_tatrf_intpwrmeas_gsm_band,
v_tatrf_intpwrmeas_gsm_req_seq.band_LSB);
switch(v_tatrf_intpwrmeas_gsm_band) {
case INFO_NO_GSM:
DEREF_PTR_SET(value, u32, 0);
break;
case INFO_GSM850:
DEREF_PTR_SET(value, u32, 1);
break;
case INFO_GSM900:
DEREF_PTR_SET(value, u32, 2);
break;
case INFO_GSM1800:
DEREF_PTR_SET(value, u32, 3);
break;
case INFO_GSM1900:
DEREF_PTR_SET(value, u32, 4);
break;
default:
vl_Error = TAT_BAD_REQ;
break;
}
break;
case ACT_TX2G5_INTPWRMEASURE_CHANNEL:
DEREF_PTR_SET(value, u16, v_tatrf_intpwrmeas_gsm_req_seq.channel);
break;
case ACT_TX2G5_INTPWRMEASURE_MODE:
DEREF_PTR_SET(value, u8, HIGHBYTE(v_tatrf_intpwrmeas_gsm_req_seq.mode_filler));
break;
case ACT_TX2G5_INTPWRMEASURE_PCL:
DEREF_PTR_SET(value, u16, v_tatrf_intpwrmeas_gsm_req_seq.powerlevel);
break;
case ACT_TX2G5_INTPWRMEASURE_FWDPOWER:
DEREF_PTR_SET(value, float,
RF_UNQ(DEREF_PTR(&v_tatrf_intpwrmeas_gsm_resp_seq.transpower, int16), 6));
break;
case ACT_TX2G5_INTPWRMEASURE_REFPOWER:
DEREF_PTR_SET(value, float,
RF_UNQ(DEREF_PTR(&v_tatrf_intpwrmeas_gsm_resp_seq.recvpower, int16), 6));
break;
case ACT_TX2G5_INTPWRMEASURE_FWDADC:
DEREF_PTR_SET(value, u16, v_tatrf_intpwrmeas_gsm_resp_seq.transpower_inraw);
break;
case ACT_TX2G5_INTPWRMEASURE_REFADC:
DEREF_PTR_SET(value, u16, v_tatrf_intpwrmeas_gsm_resp_seq.recvpower_inraw);
break;
case ACT_TX2G5_INTPWRMEASURE_STATUS:
DEREF_PTR_SET(value, u8, LOWBYTE(v_tatrf_intpwrmeas_gsm_resp_seq.filler_status));
break;
default:
vl_Error = TAT_BAD_REQ;
break;
}
return vl_Error;
}
/*
update navigation for normal mission waypoints. Return true when the
waypoint is complete
*/
bool Plane::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
{
steer_state.hold_course_cd = -1;
// depending on the pass by flag either go to waypoint in regular manner or
// fly past it for set distance along the line of waypoints
Location flex_next_WP_loc = next_WP_loc;
uint8_t cmd_passby = HIGHBYTE(cmd.p1); // distance in meters to pass beyond the wp
uint8_t cmd_acceptance_distance = LOWBYTE(cmd.p1); // radius in meters to accept reaching the wp
if (cmd_passby > 0) {
float dist = get_distance(prev_WP_loc, flex_next_WP_loc);
if (!is_zero(dist)) {
float factor = (dist + cmd_passby) / dist;
flex_next_WP_loc.lat = flex_next_WP_loc.lat + (flex_next_WP_loc.lat - prev_WP_loc.lat) * (factor - 1.0f);
flex_next_WP_loc.lng = flex_next_WP_loc.lng + (flex_next_WP_loc.lng - prev_WP_loc.lng) * (factor - 1.0f);
}
}
if (auto_state.no_crosstrack) {
nav_controller->update_waypoint(current_loc, flex_next_WP_loc);
} else {
nav_controller->update_waypoint(prev_WP_loc, flex_next_WP_loc);
}
// see if the user has specified a maximum distance to waypoint
// If override with p3 - then this is not used as it will overfly badly
if (g.waypoint_max_radius > 0 &&
auto_state.wp_distance > (uint16_t)g.waypoint_max_radius) {
if (location_passed_point(current_loc, prev_WP_loc, flex_next_WP_loc)) {
// this is needed to ensure completion of the waypoint
if (cmd_passby == 0) {
prev_WP_loc = current_loc;
}
}
return false;
}
float acceptance_distance_m = 0; // default to: if overflown - let it fly up to the point
if (cmd_acceptance_distance > 0) {
// allow user to override acceptance radius
acceptance_distance_m = cmd_acceptance_distance;
} else if (cmd_passby == 0) {
acceptance_distance_m = nav_controller->turn_distance(g.waypoint_radius, auto_state.next_turn_angle);
} else {
}
if (auto_state.wp_distance <= acceptance_distance_m) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Reached waypoint #%i dist %um",
(unsigned)mission.get_current_nav_cmd().index,
(unsigned)get_distance(current_loc, flex_next_WP_loc));
return true;
}
// have we flown past the waypoint?
if (location_passed_point(current_loc, prev_WP_loc, flex_next_WP_loc)) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Passed waypoint #%i dist %um",
(unsigned)mission.get_current_nav_cmd().index,
(unsigned)get_distance(current_loc, flex_next_WP_loc));
return true;
}
return false;
}
