int main(int argc, char*argv[])
{
char *conf_file_name = NULL;
FILE *conf_file_pointer = NULL;
char line[LINE_MAX] = {'\0'};
char *tokens[TOKEN_MAX] = {NULL};
int count = 0;
gateway_create_params gateway_device = {NULL, NULL};
gateway_handle gateway = NULL;
int return_value = E_FAILURE;
LOG_DEBUG(("DEBUG: Number of arguments are: %d\n", argc));
if(argc<2)
{
LOG_ERROR(("ERROR: Please provide configuration file name\n"));
return (0);
}
conf_file_name = argv[1];
LOG_DEBUG(("DEBUG: Configuration File Name is %s\n", conf_file_name));
conf_file_pointer = fopen(conf_file_name, "r");
if(!conf_file_pointer)
{
LOG_ERROR(("ERROR: Error in opening configuration file\n"));
return (0);
}
/* Read line */
if(fgets(line, LINE_MAX, conf_file_pointer) == NULL)
{
LOG_DEBUG(("DEBUG: Cleanup and return\n"));
fclose(conf_file_pointer);
LOG_ERROR(("ERROR: Wrong configuration file\n"));
return (0);
}
str_tokenize(line, ":\n\r", tokens, &count);
if(count<2)
{
LOG_ERROR(("Wrong configuration file\n"));
fclose(conf_file_pointer);
return (0);
}
str_copy(&gateway_device.gateway_ip_address, tokens[0]);
str_copy(&gateway_device.gateway_port_no, tokens[1]);
LOG_DEBUG(("IP Address: %s\n", gateway_device.gateway_ip_address));
LOG_DEBUG(("Port No: %s\n", gateway_device.gateway_port_no));
LOG_GATEWAY(("------------------------------------------------\n"));
return_value = create_gateway(&gateway, &gateway_device);
if(E_SUCCESS != return_value)
{
LOG_ERROR(("ERROR: Unable to create gateway\n"));
free(gateway_device.gateway_ip_address);
free(gateway_device.gateway_port_no);
fclose(conf_file_pointer);
return (0);
}
LOG_ERROR(("Gateway started successfully\n"));
int choice, interval;
while(1)
{
printf("-------------------Menu----------------\n");
printf("1.Change Sensor interval\n");
printf("2.Exit\n");
printf("Enter your choice: ");
scanf("%d", &choice);
if(choice==2)
break;
switch(choice)
{
case 1:
print_sensors(gateway);
printf("Enter sensor id: ");
scanf("%d", &choice);
printf("Enter new interval: ");
scanf("%d", &interval);
return_value = set_interval(gateway, choice, interval);
if(E_SUCCESS != return_value)
{
printf("Unable to set the interval\n");
}
break;
default:
printf("Enter valid choice...\n");
}
}
delete_gateway(gateway);
LOG_GATEWAY(("-------------------------------------------------\n"));
free(gateway_device.gateway_ip_address);
free(gateway_device.gateway_port_no);
fclose(conf_file_pointer);
logger_close();
return (0);
}
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
for (int i=0; i < 10000; i++)
{
float alpha = 10000;
int ret = api.send_position_estimated(alpha, alpha, alpha, 0, 0, 0);
print_sensors(&api);
usleep(100000);
}
// for (int i=0; i < 1000; i++)
// {
// int alpha = 200;
// int ret = api.send_position_observed(alpha, alpha, alpha, 0, 0, 0);
// printf("POSITON OBSERVED SENDED, RETURN: %d\n", ret);
// sleep(0.2);
// }
sleep(1);
// --------------------------------------------------------------------------
// 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;
}
