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";
}
}
}
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;
}
bool Plane::verify_loiter_turns()
{
bool result = false;
uint16_t radius = HIGHBYTE(mission.get_current_nav_cmd().p1);
update_loiter(radius);
// LOITER_TURNS makes no sense as VTOL
auto_state.vtol_loiter = false;
if (condition_value != 0) {
// primary goal, loiter time
if (loiter.sum_cd > loiter.total_cd && loiter.sum_cd > 1) {
// primary goal completed, initialize secondary heading goal
condition_value = 0;
result = verify_loiter_heading(true);
}
} else {
// secondary goal, loiter to heading
result = verify_loiter_heading(false);
}
if (result) {
gcs_send_text(MAV_SEVERITY_INFO,"Loiter orbits complete");
}
return result;
}
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());
}
}
// do_circle - initiate moving in a circle
void Copter::do_circle(const AP_Mission::Mission_Command& cmd)
{
Location_Class circle_center(cmd.content.location);
// default lat/lon to current position if not provided
// To-Do: use stopping point or position_controller's target instead of current location to avoid jerk?
if (circle_center.lat == 0 && circle_center.lng == 0) {
circle_center.lat = current_loc.lat;
circle_center.lng = current_loc.lng;
}
// default target altitude to current altitude if not provided
if (circle_center.alt == 0) {
int32_t curr_alt;
if (current_loc.get_alt_cm(circle_center.get_alt_frame(),curr_alt)) {
// circle altitude uses frame from command
circle_center.set_alt_cm(curr_alt,circle_center.get_alt_frame());
} else {
// default to current altitude above origin
circle_center.set_alt_cm(current_loc.alt, current_loc.get_alt_frame());
Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
}
}
// calculate radius
uint8_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1
// move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge
auto_circle_movetoedge_start(circle_center, circle_radius_m);
}
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 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));
}
}
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));
}
}
// do_circle - initiate moving in a circle
void Copter::do_circle(const AP_Mission::Mission_Command& cmd)
{
Vector3f curr_pos = inertial_nav.get_position();
Vector3f circle_center = pv_location_to_vector(cmd.content.location);
uint8_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1
bool move_to_edge_required = false;
// set target altitude if not provided
if (cmd.content.location.alt == 0) {
circle_center.z = curr_pos.z;
} else {
move_to_edge_required = true;
}
// set lat/lon position if not provided
// To-Do: use previous command's destination if it was a straight line or spline waypoint command
if (cmd.content.location.lat == 0 && cmd.content.location.lng == 0) {
circle_center.x = curr_pos.x;
circle_center.y = curr_pos.y;
} else {
move_to_edge_required = true;
}
// set circle controller's center
circle_nav.set_center(circle_center);
// set circle radius
if (circle_radius_m != 0) {
circle_nav.set_radius((float)circle_radius_m * 100.0f);
}
// check if we need to move to edge of circle
if (move_to_edge_required) {
// move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge
auto_circle_movetoedge_start();
} else {
// start circling
auto_circle_start();
}
}
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();
}
// 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;
}
}
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;
}
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
TIMIF = 0; //Clear interrupt flags
// 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;
}
