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; }