void
commands(Autopilot_Interface &api)
{
// --------------------------------------------------------------------------
// START OFFBOARD MODE
// --------------------------------------------------------------------------
api.enable_offboard_control();
usleep(100); // give some time to let it sink in
// now the autopilot is accepting setpoint commands
// --------------------------------------------------------------------------
// SEND OFFBOARD COMMANDS
// --------------------------------------------------------------------------
printf("SEND OFFBOARD COMMANDS\n");
// initialize command data strtuctures
mavlink_set_position_target_local_ned_t sp;
mavlink_set_position_target_local_ned_t ip = api.initial_position;
// autopilot_interface.h provides some helper functions to build the command
// Example 1 - Set Velocity
// set_velocity( -1.0 , // [m/s]
// -1.0 , // [m/s]
// 0.0 , // [m/s]
// sp );
// Example 2 - Set Position
set_position( ip.x - 5.0 , // [m]
ip.y - 5.0 , // [m]
ip.z , // [m]
sp );
// Example 1.2 - Append Yaw Command
set_yaw( ip.yaw , // [rad]
sp );
// SEND THE COMMAND
api.update_setpoint(sp);
// NOW pixhawk will try to move
// Wait for 8 seconds, check position
for (int i=0; i < 8; i++)
{
mavlink_local_position_ned_t pos = api.current_messages.local_position_ned;
printf("%i CURRENT POSITION XYZ = [ % .4f , % .4f , % .4f ] \n", i, pos.x, pos.y, pos.z);
sleep(1);
}
printf("\n");
// --------------------------------------------------------------------------
// STOP OFFBOARD MODE
// --------------------------------------------------------------------------
api.disable_offboard_control();
// now pixhawk isn't listening to setpoint commands
// --------------------------------------------------------------------------
// GET A MESSAGE
// --------------------------------------------------------------------------
printf("READ SOME MESSAGES \n");
// copy current messages
Mavlink_Messages messages = api.current_messages;
// local position in ned frame
mavlink_local_position_ned_t pos = messages.local_position_ned;
printf("Got message LOCAL_POSITION_NED (spec: https://pixhawk.ethz.ch/mavlink/#LOCAL_POSITION_NED)\n");
printf(" pos (NED): %f %f %f (m)\n", pos.x, pos.y, pos.z );
// hires imu
mavlink_highres_imu_t imu = messages.highres_imu;
printf("Got message HIGHRES_IMU (spec: https://pixhawk.ethz.ch/mavlink/#HIGHRES_IMU)\n");
printf(" ap time: %lu \n", imu.time_usec);
printf(" acc (NED): % f % f % f (m/s^2)\n", imu.xacc , imu.yacc , imu.zacc );
printf(" gyro (NED): % f % f % f (rad/s)\n", imu.xgyro, imu.ygyro, imu.zgyro);
printf(" mag (NED): % f % f % f (Ga)\n" , imu.xmag , imu.ymag , imu.zmag );
printf(" baro: %f (mBar) \n" , imu.abs_pressure);
printf(" altitude: %f (m) \n" , imu.pressure_alt);
printf(" temperature: %f C \n" , imu.temperature );
printf("\n");
// --------------------------------------------------------------------------
// END OF COMMANDS
// --------------------------------------------------------------------------
return;
}
void commands(Autopilot_Interface &api,
const std::vector<float> &xSetPoints, const std::vector<float> &ySetPoints,
VideoCapture &cam, VideoWriter &video) {
// <NOTE: LOCAL AXIS SYSTEM IS NED (NORTH EAST DOWN) SO POSITIVE Z SETPOINT IS LOSS IN ALTITUDE>
float setAlt = 7.0; //[m] set set point altitude above initial altitude
float setTolerance = 1.0; //tolerance for set point (how close before its cleared)
uint8_t ndx(0);
std::vector<Vec3f> circles; //vector for circles found by OpenCV
usleep(100); // give some time to let it sink in
bool setPointReached = false, ballFound = false;
//uncomment to allow logging to SD card
std::ofstream Local_Pos; //#32, 85 (target)
std::ofstream Global_Pos; //#33, 87 (target)
std::ofstream Attitude; //#30
std::ofstream HR_IMU; //#105 HIGHRES_IMU
// initialize command data structures
mavlink_set_position_target_local_ned_t sp;
mavlink_set_position_target_local_ned_t ip = api.initial_position;
//open log files and append header line
genDatalogs(Local_Pos, Global_Pos, Attitude, HR_IMU, api, 30);
genDatalogs(Local_Pos, Global_Pos, Attitude, HR_IMU, api, 20);
genDatalogs(Local_Pos, Global_Pos, Attitude, HR_IMU, api, 2);
assert(xSetPoints.size() == ySetPoints.size());
for (ndx = 0; ndx < xSetPoints.size(); ndx++) {
// <TODO: Fix the mix up with x,y and N,E>
set_position(ySetPoints[ndx], xSetPoints[ndx], ip.z - setAlt, sp);
api.update_setpoint(sp);
setPointReached = false;
genDatalogs(Local_Pos, Global_Pos, Attitude, HR_IMU, api, 1);
//loop until set point is reached
while (!setPointReached) {
mavlink_local_position_ned_t lpos = api.current_messages.local_position_ned;
// <TODO: Insert CV Code here>
ballFound = checkFrame(cam, video, circles);
// if ball is found, update setpoint to current position and return
if (ballFound) {
cam.release();
set_position(lpos.x, lpos.y, lpos.z, sp);
api.update_setpoint(sp);
genDatalogs(Local_Pos, Global_Pos, Attitude, HR_IMU, api, 1);
return;
}
// <TODO: Implement position check to occur less often (lower priority) >
if (abs(lpos.x - sp.x) < setTolerance && abs(lpos.y - sp.y) < setTolerance && abs(lpos.z - sp.z) < setTolerance) {
setPointReached = true;
break;
}
}
genDatalogs(Local_Pos, Global_Pos, Attitude, HR_IMU, api, 10);
genDatalogs(Local_Pos, Global_Pos, Attitude, HR_IMU, api, 1);
sleep(1); //wait a second for vehicle to catch up
}
//set final yaw to 0 radians (expected to be pointing north)
set_yaw(0.0, sp);
api.update_setpoint(sp);
genDatalogs(Local_Pos, Global_Pos, Attitude, HR_IMU, api, 20);
genDatalogs(Local_Pos, Global_Pos, Attitude, HR_IMU, api, 31);
return;
}
void
commands(Autopilot_Interface &api)
{
// --------------------------------------------------------------------------
// START OFFBOARD MODE
// --------------------------------------------------------------------------
api.enable_offboard_control();
usleep(100); // give some time to let it sink in
// Prepare our context and socket
zmq::context_t context (1);
zmq::socket_t socket (context, ZMQ_REP);
socket.bind ("tcp://*:5555");
while (true) {
int json_size = 8192;
char json[json_size];
char buffer[json_size];
zmq::message_t request;
// Wait for next request from client
socket.recv (&request);
std::cout << "Received Hello" << std::endl;
Mavlink_Messages messages = api.current_messages;
// Do some 'work'
#define SPITF(X,Y) {sprintf(buffer, ",\"%s\" : \"%f\"\n", X, messages.Y); strncat(json, buffer, json_size);}
#define SPITD(X,Y) {sprintf(buffer, ",\"%s\" : \"%d\"\n", X, messages.Y); strncat(json, buffer, json_size);}
sprintf(json, "{ \"version\" : \"1.0\"\n");
SPITF("lat", global_position_int.lat / 10000000.0);
SPITF("lon", global_position_int.lon / 10000000.0);
SPITF("alt", global_position_int.alt / 1000.0);
SPITF("relative_alt", global_position_int.relative_alt / 1000.0);
sprintf(buffer, "}"); strncat(json, buffer, json_size);
// Send reply back to client
int msglen = strlen(json);
zmq::message_t reply (msglen);
fprintf(stderr,"Send %s\n", json);
memcpy (reply.data (), json, msglen);
socket.send (reply);
}
return;
#if 0
// local position in ned frame
mavlink_local_position_ned_t pos = messages.local_position_ned;
printf("Got message LOCAL_POSITION_NED (spec: https://pixhawk.ethz.ch/mavlink/#LOCAL_POSITION_NED)\n");
printf(" pos (NED): %f %f %f (m)\n", pos.x, pos.y, pos.z );
// hires imu
mavlink_highres_imu_t imu = messages.highres_imu;
printf("Got message HIGHRES_IMU (spec: https://pixhawk.ethz.ch/mavlink/#HIGHRES_IMU)\n");
printf(" ap time: %lu \n", imu.time_usec);
printf(" acc (NED): % f % f % f (m/s^2)\n", imu.xacc , imu.yacc , imu.zacc );
printf(" gyro (NED): % f % f % f (rad/s)\n", imu.xgyro, imu.ygyro, imu.zgyro);
printf(" mag (NED): % f % f % f (Ga)\n" , imu.xmag , imu.ymag , imu.zmag );
printf(" baro: %f (mBar) \n" , imu.abs_pressure);
printf(" altitude: %f (m) \n" , imu.pressure_alt);
printf(" temperature: %f C \n" , imu.temperature );
printf("\n");
// --------------------------------------------------------------------------
// END OF COMMANDS
// --------------------------------------------------------------------------
return;
#endif
}
