int uORBTest::UnitTest::test_unadvertise() { test_note("Testing unadvertise"); //we still have the advertisements from the previous test_multi calls. for (int i = 0; i < 4; ++i) { int ret = orb_unadvertise(_pfd[i]); if (ret != PX4_OK) { return test_fail("orb_unadvertise failed (%i)", ret); } } //try to advertise and see whether we get the right instance int instance_test[4]; struct orb_test t; for (int i = 0; i < 4; ++i) { _pfd[i] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance_test[i], ORB_PRIO_MAX); if (instance_test[i] != i) { return test_fail("got wrong instance (should be %i, is %i)", i, instance_test[i]); } } for (int i = 0; i < 4; ++i) { orb_unadvertise(_pfd[i]); } return test_note("PASS unadvertise"); }
/** * Creates a fake traffic measurement with supplied parameters. * */ void Navigator::fake_traffic(const char *callsign, float distance, float direction, float traffic_heading, float altitude_diff, float hor_velocity, float ver_velocity) { double lat, lon; waypoint_from_heading_and_distance(get_global_position()->lat, get_global_position()->lon, direction, distance, &lat, &lon); float alt = get_global_position()->alt + altitude_diff; // float vel_n = get_global_position()->vel_n; // float vel_e = get_global_position()->vel_e; // float vel_d = get_global_position()->vel_d; transponder_report_s tr = {}; tr.timestamp = hrt_absolute_time(); tr.ICAO_address = 1234; tr.lat = lat; // Latitude, expressed as degrees tr.lon = lon; // Longitude, expressed as degrees tr.altitude_type = 0; tr.altitude = alt; tr.heading = traffic_heading; //-atan2(vel_e, vel_n); // Course over ground in radians tr.hor_velocity = hor_velocity; //sqrtf(vel_e * vel_e + vel_n * vel_n); // The horizontal velocity in m/s tr.ver_velocity = ver_velocity; //-vel_d; // The vertical velocity in m/s, positive is up strncpy(&tr.callsign[0], callsign, sizeof(tr.callsign)); tr.emitter_type = 0; // Type from ADSB_EMITTER_TYPE enum tr.tslc = 2; // Time since last communication in seconds tr.flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS | transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING | transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY | transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE | transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN; // Flags to indicate various statuses including valid data fields tr.squawk = 6667; orb_advert_t h = orb_advertise_queue(ORB_ID(transponder_report), &tr, transponder_report_s::ORB_QUEUE_LENGTH); (void)orb_unadvertise(h); }
MavlinkULog::~MavlinkULog() { if (_ulog_stream_ack_pub) { orb_unadvertise(_ulog_stream_ack_pub); } if (_ulog_stream_sub >= 0) { orb_unsubscribe(_ulog_stream_sub); } }
MavlinkParametersManager::~MavlinkParametersManager() { if (_uavcan_parameter_value_sub >= 0) { orb_unsubscribe(_uavcan_parameter_value_sub); } if (_uavcan_parameter_request_pub) { orb_unadvertise(_uavcan_parameter_request_pub); } }
void CameraTrigger::test() { struct vehicle_command_s cmd = {}; cmd.command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL; cmd.param5 = 1.0f; orb_advert_t pub; pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH); (void)orb_unadvertise(pub); }
MPU9250::~MPU9250() { /* make sure we are truly inactive */ stop(); _call_interval = 0; if (!_magnetometer_only) { orb_unadvertise(_accel_topic); orb_unadvertise(_gyro->_gyro_topic); } /* delete the accel subdriver */ delete _accel; /* delete the gyro subdriver */ delete _gyro; /* delete the magnetometer subdriver */ delete _mag; /* free any existing reports */ if (_accel_reports != nullptr) { delete _accel_reports; } if (_gyro_reports != nullptr) { delete _gyro_reports; } /* delete the perf counter */ perf_free(_sample_perf); perf_free(_accel_reads); perf_free(_gyro_reads); perf_free(_bad_transfers); perf_free(_bad_registers); perf_free(_good_transfers); perf_free(_reset_retries); perf_free(_duplicates); }
OutputBase::~OutputBase() { if (_vehicle_attitude_sub >= 0) { orb_unsubscribe(_vehicle_attitude_sub); } if (_vehicle_global_position_sub >= 0) { orb_unsubscribe(_vehicle_global_position_sub); } if (_mount_orientation_pub) { orb_unadvertise(_mount_orientation_pub); } }
int uORBTest::UnitTest::pub_test_multi2_main() { int data_next_idx = 0; const int num_instances = 3; orb_advert_t orb_pub[num_instances]; struct orb_test_medium data_topic; for (int i = 0; i < num_instances; ++i) { orb_advert_t &pub = orb_pub[i]; int idx = i; // PX4_WARN("advertise %i, t=%" PRIu64, i, hrt_absolute_time()); pub = orb_advertise_multi(ORB_ID(orb_test_medium_multi), &data_topic, &idx, ORB_PRIO_DEFAULT); if (idx != i) { _thread_should_exit = true; PX4_ERR("Got wrong instance! should be: %i, but is %i", i, idx); return -1; } } usleep(100 * 1000); int message_counter = 0, num_messages = 50 * num_instances; while (message_counter++ < num_messages) { usleep(2); //make sure the timestamps are different orb_advert_t &pub = orb_pub[data_next_idx]; data_topic.time = hrt_absolute_time(); data_topic.val = data_next_idx; orb_publish(ORB_ID(orb_test_medium_multi), pub, &data_topic); // PX4_WARN("publishing msg (idx=%i, t=%" PRIu64 ")", data_next_idx, data_topic.time); data_next_idx = (data_next_idx + 1) % num_instances; if (data_next_idx == 0) { usleep(50 * 1000); } } usleep(100 * 1000); _thread_should_exit = true; for (int i = 0; i < num_instances; ++i) { orb_unadvertise(orb_pub[i]); } return 0; }
MPU9250::~MPU9250() { /* make sure we are truly inactive */ stop(); orb_unadvertise(_accel_topic); orb_unadvertise(_gyro->_gyro_topic); /* delete the gyro subdriver */ delete _gyro; /* delete the magnetometer subdriver */ delete _mag; /* free any existing reports */ if (_accel_reports != nullptr) { delete _accel_reports; } if (_gyro_reports != nullptr) { delete _gyro_reports; } if (_accel_class_instance != -1) { unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); } /* delete the perf counter */ perf_free(_sample_perf); perf_free(_accel_reads); perf_free(_gyro_reads); perf_free(_bad_transfers); perf_free(_bad_registers); perf_free(_good_transfers); perf_free(_reset_retries); perf_free(_duplicates); }
void CameraTrigger::test() { struct vehicle_command_s cmd = { .timestamp = hrt_absolute_time(), .param5 = 1.0f, .param6 = 0.0f, .param1 = 0.0f, .param2 = 0.0f, .param3 = 0.0f, .param4 = 0.0f, .param7 = 0.0f, .command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL }; orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &cmd, vehicle_command_s::ORB_QUEUE_LENGTH); (void)orb_unadvertise(pub); }
SF1XX::~SF1XX() { /* make sure we are truly inactive */ stop(); /* free any existing reports */ if (_reports != nullptr) { delete _reports; } if (_distance_sensor_topic != nullptr) { orb_unadvertise(_distance_sensor_topic); } if (_class_instance != -1) { unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance); } /* free perf counters */ perf_free(_sample_perf); perf_free(_comms_errors); }
leddar_one::~leddar_one() { stop(); free((char *)_serial_port); if (_fd > -1) { ::close(_fd); } if (_reports) { delete _reports; } if (_topic) { orb_unadvertise(_topic); } perf_free(_sample_perf); perf_free(_collect_timeout_perf); perf_free(_comm_error); }
LIS3MDL::~LIS3MDL() { /* make sure we are truly inactive */ stop(); if (_mag_topic != nullptr) { orb_unadvertise(_mag_topic); } if (_reports != nullptr) { delete _reports; } if (_class_instance != -1) { unregister_class_devname(MAG_BASE_DEVICE_PATH, _class_instance); } // free perf counters perf_free(_sample_perf); perf_free(_comms_errors); perf_free(_range_errors); perf_free(_conf_errors); }
int uORBTest::UnitTest::pub_test_queue_main() { struct orb_test_medium t; orb_advert_t ptopic; const unsigned int queue_size = 50; t.val = 0; if ((ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue_poll), &t, queue_size)) == nullptr) { _thread_should_exit = true; return test_fail("advertise failed: %d", errno); } int message_counter = 0, num_messages = 20 * queue_size; ++t.val; while (message_counter < num_messages) { //simulate burst int burst_counter = 0; while (burst_counter++ < queue_size / 2 + 7) { //make interval non-boundary aligned orb_publish(ORB_ID(orb_test_medium_queue_poll), ptopic, &t); ++t.val; } message_counter += burst_counter; usleep(20 * 1000); //give subscriber a chance to catch up } _num_messages_sent = t.val; usleep(100 * 1000); _thread_should_exit = true; orb_unadvertise(ptopic); return 0; }
OutputMavlink::~OutputMavlink() { if (_vehicle_command_pub) { orb_unadvertise(_vehicle_command_pub); } }
int uORBTest::UnitTest::test_queue() { test_note("Testing orb queuing"); struct orb_test_medium t, u; int sfd; orb_advert_t ptopic; bool updated; sfd = orb_subscribe(ORB_ID(orb_test_medium_queue)); if (sfd < 0) { return test_fail("subscribe failed: %d", errno); } const unsigned int queue_size = 11; t.val = 0; ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue), &t, queue_size); if (ptopic == nullptr) { return test_fail("advertise failed: %d", errno); } orb_check(sfd, &updated); if (!updated) { return test_fail("update flag not set"); } if (PX4_OK != orb_copy(ORB_ID(orb_test_medium_queue), sfd, &u)) { return test_fail("copy(1) failed: %d", errno); } if (u.val != t.val) { return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val); } orb_check(sfd, &updated); if (updated) { return test_fail("spurious updated flag"); } #define CHECK_UPDATED(element) \ orb_check(sfd, &updated); \ if (!updated) { \ return test_fail("update flag not set, element %i", element); \ } #define CHECK_NOT_UPDATED(element) \ orb_check(sfd, &updated); \ if (updated) { \ return test_fail("update flag set, element %i", element); \ } #define CHECK_COPY(i_got, i_correct) \ orb_copy(ORB_ID(orb_test_medium_queue), sfd, &u); \ if (i_got != i_correct) { \ return test_fail("got wrong element from the queue (got %i, should be %i)", i_got, i_correct); \ } //no messages in the queue anymore test_note(" Testing to write some elements..."); for (unsigned int i = 0; i < queue_size - 2; ++i) { t.val = i; orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t); } for (unsigned int i = 0; i < queue_size - 2; ++i) { CHECK_UPDATED(i); CHECK_COPY(u.val, i); } CHECK_NOT_UPDATED(queue_size); test_note(" Testing overflow..."); int overflow_by = 3; for (unsigned int i = 0; i < queue_size + overflow_by; ++i) { t.val = i; orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t); } for (unsigned int i = 0; i < queue_size; ++i) { CHECK_UPDATED(i); CHECK_COPY(u.val, i + overflow_by); } CHECK_NOT_UPDATED(queue_size); test_note(" Testing underflow..."); for (unsigned int i = 0; i < queue_size; ++i) { CHECK_NOT_UPDATED(i); CHECK_COPY(u.val, queue_size + overflow_by - 1); } t.val = 943; orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t); CHECK_UPDATED(-1); CHECK_COPY(u.val, t.val); #undef CHECK_COPY #undef CHECK_UPDATED #undef CHECK_NOT_UPDATED orb_unadvertise(ptopic); return test_note("PASS orb queuing"); }
void *multirotor_attitude_control_thread_main() { /* welcome user */ fprintf (stdout, "Multirotor attitude controller started\n"); fflush(stdout); int i; bool_t updated, rates_sp_updated, reset_integral; float rates[3]; /* store last control mode to detect mode switches */ bool_t control_yaw_position = 1 /* true */; bool_t reset_yaw_sp = 1 /* true */; /* declare and safely initialize all structs */ struct parameter_update_s update; struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; memset(&att_sp, 0, sizeof(att_sp)); struct offboard_control_setpoint_s offboard_sp; memset(&offboard_sp, 0, sizeof(offboard_sp)); struct vehicle_control_flags_s control_flags; memset(&control_flags, 0, sizeof(control_flags)); struct manual_control_setpoint_s manual; memset(&manual, 0, sizeof(manual)); struct sensor_combined_s sensor; memset(&sensor, 0, sizeof(sensor)); struct vehicle_rates_setpoint_s rates_sp; memset(&rates_sp, 0, sizeof(rates_sp)); struct vehicle_status_s status; memset(&status, 0, sizeof(status)); struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); /* subscribe */ orb_subscr_t vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); if (vehicle_attitude_sub < 0) { fprintf (stderr, "Attitude controller thread failed to subscribe to vehicle_attitude topic\n"); exit(-1); } orb_subscr_t parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); if (parameter_update_sub < 0) { fprintf (stderr, "Attitude controller thread failed to subscribe to parameter_update topic\n"); exit(-1); } orb_subscr_t vehicle_attitude_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); if (vehicle_attitude_setpoint_sub < 0) { fprintf (stderr, "Attitude controller thread failed to subscribe to vehicle_attitude_setpoint topic\n"); exit(-1); } orb_subscr_t offboard_control_setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); if (offboard_control_setpoint_sub < 0) { fprintf (stderr, "Attitude controller thread failed to subscribe to offboard_control_setpoint topic\n"); exit(-1); } orb_subscr_t vehicle_control_flags_sub = orb_subscribe(ORB_ID(vehicle_control_flags)); if (vehicle_control_flags_sub < 0) { fprintf (stderr, "Attitude controller thread failed to subscribe to vehicle_control_flags topic\n"); exit(-1); } orb_subscr_t manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); if (manual_control_setpoint_sub < 0) { fprintf (stderr, "Attitude controller thread failed to subscribe to manual_control_setpoint topic\n"); exit(-1); } orb_subscr_t sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); if (sensor_combined_sub < 0) { fprintf (stderr, "Attitude controller thread failed to subscribe to sensor_combined topic\n"); exit(-1); } orb_subscr_t vehicle_rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); if (vehicle_rates_setpoint_sub < 0) { fprintf (stderr, "Attitude controller thread failed to subscribe to vehicle_rates_setpoint topic\n"); exit(-1); } orb_subscr_t vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); if (vehicle_status_sub < 0) { fprintf (stderr, "Attitude controller thread failed to subscribe to vehicle_status topic\n"); exit(-1); } /* publish actuator controls */ for (i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { actuators.control[i] = 0.0f; } orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); if (actuator_pub == -1) { fprintf (stderr, "Comunicator thread failed to advertise the actutor_controls topic\n"); exit (-1); } orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint)); if (att_sp_pub == -1) { fprintf (stderr, "Comunicator thread failed to advertise the vehicle_attitude_setpoint topic\n"); exit (-1); } orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint)); if (rates_sp_pub == -1) { fprintf (stderr, "Comunicator thread failed to advertise the vehicle_rates_setpoint topic\n"); exit (-1); } orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); while (!_shutdown_all_systems) { /* wait for a sensor update, check for exit condition every 500 ms */ updated = orb_poll(ORB_ID(vehicle_attitude), vehicle_attitude_sub, 500000); /* timed out - periodic check for _shutdown_all_systems, etc. */ if (!updated) continue; /* this is undesirable but not much we can do - might want to flag unhappy status */ if (updated < 0) { fprintf (stderr, "Attitude controller failed to poll vehicle attitude\n"); continue; } /* only run controller if attitude changed */ /* attitude */ orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); /* parameters */ updated = orb_check (ORB_ID(parameter_update), parameter_update_sub); if (updated) { orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); /* update parameters */ } /* control mode */ updated = orb_check (ORB_ID(vehicle_control_flags), vehicle_control_flags_sub); if (updated) { orb_copy(ORB_ID(vehicle_control_flags), vehicle_control_flags_sub, &control_flags); } /* manual control setpoint */ updated = orb_check (ORB_ID(manual_control_setpoint), manual_control_setpoint_sub); if (updated) { orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual); } /* attitude setpoint */ updated = orb_check (ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub); if (updated) { orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp); } /* offboard control setpoint */ updated = orb_check (ORB_ID(offboard_control_setpoint), offboard_control_setpoint_sub); if (updated) { orb_copy(ORB_ID(offboard_control_setpoint), offboard_control_setpoint_sub, &offboard_sp); } /* vehicle status */ updated = orb_check (ORB_ID(vehicle_status), vehicle_status_sub); if (updated) { orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &status); } /* sensors */ updated = orb_check (ORB_ID(sensor_combined), sensor_combined_sub); if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); } /* set flag to safe value */ control_yaw_position = 1 /* true */; /* reset yaw setpoint if not armed */ if (!control_flags.flag_armed) { reset_yaw_sp = 1 /* true */; } /* define which input is the dominating control input */ if (control_flags.flag_control_offboard_enabled) { /* offboard inputs */ if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { rates_sp.roll = offboard_sp.p1; rates_sp.pitch = offboard_sp.p2; rates_sp.yaw = offboard_sp.p3; rates_sp.thrust = offboard_sp.p4; rates_sp.timestamp = get_absolute_time(); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); } else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) { att_sp.roll_body = offboard_sp.p1; att_sp.pitch_body = offboard_sp.p2; att_sp.yaw_body = offboard_sp.p3; att_sp.thrust = offboard_sp.p4; att_sp.timestamp = get_absolute_time(); /* publish the result to the vehicle actuators */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } /* reset yaw setpoint after offboard control */ reset_yaw_sp = 1 /* true */; } else if (control_flags.flag_control_manual_enabled) { /* manual input */ if (control_flags.flag_control_attitude_enabled) { /* control attitude, update attitude setpoint depending on mode */ if (att_sp.thrust < 0.1f) { /* no thrust, don't try to control yaw */ rates_sp.yaw = 0.0f; control_yaw_position = 0 /* false */; if (status.condition_landed) { /* reset yaw setpoint if on ground */ reset_yaw_sp = 1 /* true */; } } else { /* only move yaw setpoint if manual input is != 0 */ if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) { /* control yaw rate */ control_yaw_position = 0 /* false */; rates_sp.yaw = manual.yaw; reset_yaw_sp = 1 /* true */; // has no effect on control, just for beautiful log } else { control_yaw_position = 1 /* true */; } } if (!control_flags.flag_control_velocity_enabled) { /* update attitude setpoint if not in position control mode */ att_sp.roll_body = manual.roll; att_sp.pitch_body = manual.pitch; if (!control_flags.flag_control_climb_rate_enabled) { /* pass throttle directly if not in altitude control mode */ att_sp.thrust = manual.thrust; } } /* reset yaw setpint to current position if needed */ if (reset_yaw_sp) { att_sp.yaw_body = att.yaw; reset_yaw_sp = 0 /* false */; } /*if (motor_test_mode) { printf("testmode"); att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; att_sp.yaw_body = 0.0f; att_sp.thrust = 0.1f; }*/ att_sp.timestamp = get_absolute_time(); /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } else { /* manual rate inputs (ACRO), from RC control or joystick */ if (control_flags.flag_control_rates_enabled) { rates_sp.roll = manual.roll; rates_sp.pitch = manual.pitch; rates_sp.yaw = manual.yaw; rates_sp.thrust = manual.thrust; rates_sp.timestamp = get_absolute_time(); } /* reset yaw setpoint after ACRO */ reset_yaw_sp = 1 /* true */; } } else { if (!control_flags.flag_control_auto_enabled) { /* no control, try to stay on place */ if (!control_flags.flag_control_velocity_enabled) { /* no velocity control, reset attitude setpoint */ att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; att_sp.timestamp = get_absolute_time(); orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } } /* reset yaw setpoint after non-manual control */ reset_yaw_sp = 1 /* true */; } /* check if we should we reset integrals */ reset_integral = !control_flags.flag_armed || att_sp.thrust < 0.1f; // TODO use landed status instead of throttle /* run attitude controller if needed */ if (control_flags.flag_control_attitude_enabled) { multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); } /* run rates controller if needed */ if (control_flags.flag_control_rates_enabled) { /* get current rate setpoint */ rates_sp_updated = orb_check (ORB_ID(vehicle_rates_setpoint), vehicle_rates_setpoint_sub); if (rates_sp_updated) { orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rates_setpoint_sub, &rates_sp); } /* apply controller */ rates[0] = att.roll_rate; rates[1] = att.pitch_rate; rates[2] = att.yaw_rate; multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral); } else { /* rates controller disabled, set actuators to zero for safety */ actuators.control[0] = 0.0f; actuators.control[1] = 0.0f; actuators.control[2] = 0.0f; actuators.control[3] = 0.0f; } /* fill in manual control values */ actuators.control[4] = manual.flaps; //actuators.control[5] = manual.aux1; //actuators.control[6] = manual.aux2; //actuators.control[7] = manual.aux3; orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); } fprintf (stdout, "INFO: Attitude controller exiting, disarming motors\n"); fflush (stdout); /* kill all outputs */ for (i = 0; i < NUM_ACTUATOR_CONTROLS; i++) actuators.control[i] = 0.0f; orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); /* * do unsubscriptions */ orb_unsubscribe(ORB_ID(vehicle_attitude), vehicle_attitude_sub, pthread_self()); orb_unsubscribe(ORB_ID(parameter_update), parameter_update_sub, pthread_self()); orb_unsubscribe(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, pthread_self()); orb_unsubscribe(ORB_ID(offboard_control_setpoint), offboard_control_setpoint_sub, pthread_self()); orb_unsubscribe(ORB_ID(vehicle_control_flags), vehicle_control_flags_sub, pthread_self()); orb_unsubscribe(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, pthread_self()); orb_unsubscribe(ORB_ID(sensor_combined), sensor_combined_sub, pthread_self()); orb_unsubscribe(ORB_ID(vehicle_rates_setpoint), vehicle_rates_setpoint_sub, pthread_self()); orb_unsubscribe(ORB_ID(vehicle_status), vehicle_status_sub, pthread_self()); /* * do unadvertises */ orb_unadvertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, pthread_self()); orb_unadvertise(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, pthread_self()); orb_unadvertise(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, pthread_self()); return 0; }
int uORBTest::UnitTest::test_single() { test_note("try single-topic support"); struct orb_test t, u; int sfd; orb_advert_t ptopic; bool updated; t.val = 0; ptopic = orb_advertise(ORB_ID(orb_test), &t); if (ptopic == nullptr) { return test_fail("advertise failed: %d", errno); } test_note("publish handle 0x%08x", ptopic); sfd = orb_subscribe(ORB_ID(orb_test)); if (sfd < 0) { return test_fail("subscribe failed: %d", errno); } test_note("subscribe fd %d", sfd); u.val = 1; if (PX4_OK != orb_copy(ORB_ID(orb_test), sfd, &u)) { return test_fail("copy(1) failed: %d", errno); } if (u.val != t.val) { return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val); } if (PX4_OK != orb_check(sfd, &updated)) { return test_fail("check(1) failed"); } if (updated) { return test_fail("spurious updated flag"); } t.val = 2; test_note("try publish"); if (PX4_OK != orb_publish(ORB_ID(orb_test), ptopic, &t)) { return test_fail("publish failed"); } if (PX4_OK != orb_check(sfd, &updated)) { return test_fail("check(2) failed"); } if (!updated) { return test_fail("missing updated flag"); } if (PX4_OK != orb_copy(ORB_ID(orb_test), sfd, &u)) { return test_fail("copy(2) failed: %d", errno); } if (u.val != t.val) { return test_fail("copy(2) mismatch: %d expected %d", u.val, t.val); } orb_unsubscribe(sfd); int ret = orb_unadvertise(ptopic); if (ret != PX4_OK) { return test_fail("orb_unadvertise failed: %i", ret); } return test_note("PASS single-topic test"); }
void GPS::task_main() { /* open the serial port */ _serial_fd = ::open(_port, O_RDWR | O_NOCTTY); if (_serial_fd < 0) { PX4_ERR("GPS: failed to open serial port: %s err: %d", _port, errno); /* tell the dtor that we are exiting, set error code */ _task = -1; px4_task_exit(1); } #ifndef __PX4_QURT // TODO: this call is not supported on Snapdragon just yet. // However it seems to be nonblocking anyway and working. int flags = fcntl(_serial_fd, F_GETFL, 0); fcntl(_serial_fd, F_SETFL, flags | O_NONBLOCK); #endif for (int i = 0; i < _orb_inject_data_fd_count; ++i) { _orb_inject_data_fd[i] = orb_subscribe_multi(ORB_ID(gps_inject_data), i); } initializeCommunicationDump(); uint64_t last_rate_measurement = hrt_absolute_time(); unsigned last_rate_count = 0; /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { if (_fake_gps) { _report_gps_pos.timestamp = hrt_absolute_time(); _report_gps_pos.lat = (int32_t)47.378301e7f; _report_gps_pos.lon = (int32_t)8.538777e7f; _report_gps_pos.alt = (int32_t)1200e3f; _report_gps_pos.s_variance_m_s = 10.0f; _report_gps_pos.c_variance_rad = 0.1f; _report_gps_pos.fix_type = 3; _report_gps_pos.eph = 0.9f; _report_gps_pos.epv = 1.8f; _report_gps_pos.vel_n_m_s = 0.0f; _report_gps_pos.vel_e_m_s = 0.0f; _report_gps_pos.vel_d_m_s = 0.0f; _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); _report_gps_pos.cog_rad = 0.0f; _report_gps_pos.vel_ned_valid = true; /* no time and satellite information simulated */ publish(); usleep(2e5); } else { if (_helper != nullptr) { delete(_helper); /* set to zero to ensure parser is not used while not instantiated */ _helper = nullptr; } switch (_mode) { case GPS_DRIVER_MODE_UBX: _helper = new GPSDriverUBX(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info); break; case GPS_DRIVER_MODE_MTK: _helper = new GPSDriverMTK(&GPS::callback, this, &_report_gps_pos); break; case GPS_DRIVER_MODE_ASHTECH: _helper = new GPSDriverAshtech(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info); break; default: break; } /* the Ashtech driver lies about successful configuration and the * MTK driver is not well tested, so we really only trust the UBX * driver for an advance publication */ if (_helper->configure(_baudrate, GPSHelper::OutputMode::GPS) == 0) { /* reset report */ memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); if (_mode == GPS_DRIVER_MODE_UBX) { /* Publish initial report that we have access to a GPS, * but set all critical state fields to indicate we have * no valid position lock */ /* reset the timestamp for data, because we have no data yet */ _report_gps_pos.timestamp = 0; _report_gps_pos.timestamp_time_relative = 0; /* set a massive variance */ _report_gps_pos.eph = 10000.0f; _report_gps_pos.epv = 10000.0f; _report_gps_pos.fix_type = 0; publish(); /* GPS is obviously detected successfully, reset statistics */ _helper->resetUpdateRates(); } int helper_ret; while ((helper_ret = _helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) { if (helper_ret & 1) { publish(); last_rate_count++; } if (_p_report_sat_info && (helper_ret & 2)) { publishSatelliteInfo(); } /* measure update rate every 5 seconds */ if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { float dt = (float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f; _rate = last_rate_count / dt; _rate_rtcm_injection = _last_rate_rtcm_injection_count / dt; last_rate_measurement = hrt_absolute_time(); last_rate_count = 0; _last_rate_rtcm_injection_count = 0; _helper->storeUpdateRates(); _helper->resetUpdateRates(); } if (!_healthy) { // Helpful for debugging, but too verbose for normal ops // const char *mode_str = "unknown"; // // switch (_mode) { // case GPS_DRIVER_MODE_UBX: // mode_str = "UBX"; // break; // // case GPS_DRIVER_MODE_MTK: // mode_str = "MTK"; // break; // // case GPS_DRIVER_MODE_ASHTECH: // mode_str = "ASHTECH"; // break; // // default: // break; // } // // PX4_WARN("module found: %s", mode_str); _healthy = true; } //check for disarming if (_vehicle_status_sub != -1 && _dump_from_gps_device_fd != -1) { bool updated; orb_check(_vehicle_status_sub, &updated); if (updated) { vehicle_status_s vehicle_status; orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &vehicle_status); bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) || (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR); if (armed) { _is_armed = true; } else if (_is_armed) { //disable communication dump when disarming close(_dump_from_gps_device_fd); _dump_from_gps_device_fd = -1; close(_dump_to_gps_device_fd); _dump_to_gps_device_fd = -1; _is_armed = false; } } } } if (_healthy) { PX4_WARN("GPS module lost"); _healthy = false; _rate = 0.0f; _rate_rtcm_injection = 0.0f; } } /* select next mode */ switch (_mode) { case GPS_DRIVER_MODE_UBX: _mode = GPS_DRIVER_MODE_MTK; break; case GPS_DRIVER_MODE_MTK: _mode = GPS_DRIVER_MODE_ASHTECH; break; case GPS_DRIVER_MODE_ASHTECH: _mode = GPS_DRIVER_MODE_UBX; break; default: break; } } } PX4_WARN("exiting"); for (size_t i = 0; i < _orb_inject_data_fd_count; ++i) { orb_unsubscribe(_orb_inject_data_fd[i]); _orb_inject_data_fd[i] = -1; } if (_dump_to_gps_device_fd != -1) { close(_dump_to_gps_device_fd); _dump_to_gps_device_fd = -1; } if (_dump_from_gps_device_fd != -1) { close(_dump_from_gps_device_fd); _dump_from_gps_device_fd = -1; } if (_vehicle_status_sub != -1) { orb_unsubscribe(_vehicle_status_sub); _vehicle_status_sub = -1; } ::close(_serial_fd); orb_unadvertise(_report_gps_pos_pub); /* tell the dtor that we are exiting */ _task = -1; px4_task_exit(0); }
/** * Mission waypoint manager main function */ void *mission_waypoint_manager_thread_main(void* args) { absolute_time now; int updated; float turn_distance; /* initialize waypoint manager */ // ************************************* subscribe ****************************************** global_position_setpoint_pub = orb_advertise (ORB_ID(vehicle_global_position_setpoint)); if (global_position_setpoint_pub == -1) { fprintf (stderr, "Waypoint manager thread failed to advertise the vehicle_global_position_setpoint topic\n"); exit (-1); } global_position_set_triplet_pub = orb_advertise (ORB_ID(vehicle_global_position_set_triplet)); if (global_position_set_triplet_pub == -1) { fprintf (stderr, "Waypoint manager thread failed to advertise the vehicle_global_position_set_triplet topic\n"); exit (-1); } // ************************************* subscribe ****************************************** struct vehicle_global_position_s global_pos; orb_subscr_t global_pos_sub = orb_subscribe (ORB_ID(vehicle_global_position)); if (global_pos_sub == -1) { fprintf (stderr, "Waypoint manager thread failed to subscribe the vehicle_global_position topic\n"); exit (-1); } struct vehicle_control_flags_s control_flags; orb_subscr_t control_flags_sub = orb_subscribe(ORB_ID(vehicle_control_flags)); if (control_flags_sub < 0) { fprintf (stderr, "Waypoint manager thread failed to subscribe to vehicle_control_flags topic\n"); exit(-1); } struct navigation_capabilities_s nav_cap; orb_subscr_t nav_cap_sub = orb_subscribe (ORB_ID(navigation_capabilities)); if (nav_cap_sub == -1) { fprintf (stderr, "Waypoint manager thread failed to subscribe the navigation_capabilities topic\n"); exit (-1); } struct parameter_update_s params; orb_subscr_t param_sub = orb_subscribe (ORB_ID(parameter_update)); if (param_sub == -1) { fprintf (stderr, "Waypoint manager thread failed to subscribe the parameter_update topic\n"); exit (-1); } orb_subscr_t mission_sub = orb_subscribe (ORB_ID(mission)); if (mission_sub == -1) { fprintf (stderr, "Waypoint manager thread failed to subscribe the mission topic\n"); exit (-1); } orb_subscr_t home_sub = orb_subscribe (ORB_ID(home_position)); if (home_sub == -1) { fprintf (stderr, "Waypoint manager thread failed to subscribe the home_position topic\n"); exit (-1); } /* abort on a nonzero return value from the parameter init */ if (mission_waypoint_manager_params_init() != 0 || wp_manager_init (mission_sub, home_sub) != 0) { fprintf (stderr, "Waypoint manager exiting on startup due to an error\n"); exit (-1); } turn_distance = (is_rotary_wing)? mission_waypoint_manager_parameters.mr_turn_distance : mission_waypoint_manager_parameters.fw_turn_distance; /* welcome user */ fprintf (stdout, "Waypoint manager started\n"); fflush(stdout); /* waypoint main eventloop */ while (!_shutdown_all_systems) { // wait for the position estimation for a max of 500ms updated = orb_poll (ORB_ID(vehicle_global_position), global_pos_sub, 500000); if (updated < 0) { // that's undesiderable but there is not much we can do fprintf (stderr, "Waypoint manager thread experienced an error waiting for actuator_controls topic\n"); continue; } else if (updated == 0) { continue; } else orb_copy (ORB_ID(vehicle_global_position), global_pos_sub, (void *) &global_pos); updated = orb_check (ORB_ID(vehicle_control_flags), control_flags_sub); if (updated) { orb_copy(ORB_ID(vehicle_control_flags), control_flags_sub, &control_flags); } updated = orb_check (ORB_ID(navigation_capabilities), nav_cap_sub); if (updated) { orb_copy (ORB_ID(navigation_capabilities), nav_cap_sub, (void *) &nav_cap); turn_distance = (nav_cap.turn_distance > 0)? nav_cap.turn_distance : turn_distance; } updated = orb_check (ORB_ID(parameter_update), param_sub); if (updated) { /* clear updated flag */ orb_copy(ORB_ID(parameter_update), param_sub, ¶ms); /* update multirotor_position_control_parameters */ mission_waypoint_manager_params_update(); } if (!control_flags.flag_control_manual_enabled && control_flags.flag_control_auto_enabled) { orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &g_sp); orb_publish(ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, &triplet); } now = get_absolute_time(); /* check if waypoint has been reached against the last positions */ check_waypoints_reached(now, &global_pos, turn_distance); /* sleep 25 ms */ usleep(25000); } /* * do unsubscriptions */ if (orb_unsubscribe (ORB_ID(vehicle_global_position), global_pos_sub, pthread_self()) < 0) fprintf (stderr, "Waypoint manager thread failed to unsubscribe to vehicle_global_position topic\n"); if (orb_unsubscribe(ORB_ID(vehicle_control_flags), control_flags_sub, pthread_self()) < 0) fprintf (stderr, "Waypoint manager thread failed to unsubscribe to vehicle_control_flags topic\n"); if (orb_unsubscribe (ORB_ID(navigation_capabilities), nav_cap_sub, pthread_self()) < 0) fprintf (stderr, "Waypoint manager thread failed to unsubscribe to navigation_capabilities topic\n"); if (orb_unsubscribe (ORB_ID(parameter_update), param_sub, pthread_self()) < 0) fprintf (stderr, "Waypoint manager thread failed to unsubscribe to parameter_update topic\n"); if (orb_unsubscribe (ORB_ID(mission), mission_sub, pthread_self()) < 0) fprintf (stderr, "Waypoint manager thread failed to unsubscribe to mission topic\n"); if (orb_unsubscribe (ORB_ID(home_position), home_sub, pthread_self()) < 0) fprintf (stderr, "Waypoint manager thread failed to unsubscribe to home_position topic\n"); /* * do unadvertises */ if (orb_unadvertise (ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, pthread_self()) < 0) fprintf (stderr, "Waypoint manager thread failed to unadvertise the global_position_setpoint_pub topic\n"); if (orb_unadvertise (ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, pthread_self()) < 0) fprintf (stderr, "Waypoint manager thread failed to unadvertise the vehicle_global_position_set_triplet topic\n"); return 0; }
void GPS::task_main() { /* open the serial port */ _serial_fd = ::open(_port, O_RDWR | O_NOCTTY); if (_serial_fd < 0) { PX4_ERR("GPS: failed to open serial port: %s err: %d", _port, errno); /* tell the dtor that we are exiting, set error code */ _task = -1; px4_task_exit(1); } _orb_inject_data_fd = orb_subscribe(ORB_ID(gps_inject_data)); initializeCommunicationDump(); uint64_t last_rate_measurement = hrt_absolute_time(); unsigned last_rate_count = 0; /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { if (_fake_gps) { _report_gps_pos.timestamp = hrt_absolute_time(); _report_gps_pos.lat = (int32_t)47.378301e7f; _report_gps_pos.lon = (int32_t)8.538777e7f; _report_gps_pos.alt = (int32_t)1200e3f; _report_gps_pos.alt_ellipsoid = 10000; _report_gps_pos.s_variance_m_s = 0.5f; _report_gps_pos.c_variance_rad = 0.1f; _report_gps_pos.fix_type = 3; _report_gps_pos.eph = 0.8f; _report_gps_pos.epv = 1.2f; _report_gps_pos.hdop = 0.9f; _report_gps_pos.vdop = 0.9f; _report_gps_pos.vel_n_m_s = 0.0f; _report_gps_pos.vel_e_m_s = 0.0f; _report_gps_pos.vel_d_m_s = 0.0f; _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); _report_gps_pos.cog_rad = 0.0f; _report_gps_pos.vel_ned_valid = true; _report_gps_pos.satellites_used = 10; /* no time and satellite information simulated */ publish(); usleep(2e5); } else { if (_helper != nullptr) { delete(_helper); /* set to zero to ensure parser is not used while not instantiated */ _helper = nullptr; } switch (_mode) { case GPS_DRIVER_MODE_NONE: _mode = GPS_DRIVER_MODE_UBX; //no break case GPS_DRIVER_MODE_UBX: _helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info); break; case GPS_DRIVER_MODE_MTK: _helper = new GPSDriverMTK(&GPS::callback, this, &_report_gps_pos); break; case GPS_DRIVER_MODE_ASHTECH: _helper = new GPSDriverAshtech(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info); break; default: break; } /* the Ashtech driver lies about successful configuration and the * MTK driver is not well tested, so we really only trust the UBX * driver for an advance publication */ if (_helper->configure(_baudrate, GPSHelper::OutputMode::GPS) == 0) { /* reset report */ memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); if (_mode == GPS_DRIVER_MODE_UBX) { /* Publish initial report that we have access to a GPS, * but set all critical state fields to indicate we have * no valid position lock */ /* reset the timestamp for data, because we have no data yet */ _report_gps_pos.timestamp = 0; _report_gps_pos.timestamp_time_relative = 0; /* set a massive variance */ _report_gps_pos.eph = 10000.0f; _report_gps_pos.epv = 10000.0f; _report_gps_pos.fix_type = 0; publish(); /* GPS is obviously detected successfully, reset statistics */ _helper->resetUpdateRates(); } int helper_ret; while ((helper_ret = _helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) { if (helper_ret & 1) { publish(); last_rate_count++; } if (_p_report_sat_info && (helper_ret & 2)) { publishSatelliteInfo(); } /* measure update rate every 5 seconds */ if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { float dt = (float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f; _rate = last_rate_count / dt; _rate_rtcm_injection = _last_rate_rtcm_injection_count / dt; last_rate_measurement = hrt_absolute_time(); last_rate_count = 0; _last_rate_rtcm_injection_count = 0; _helper->storeUpdateRates(); _helper->resetUpdateRates(); } if (!_healthy) { // Helpful for debugging, but too verbose for normal ops // const char *mode_str = "unknown"; // // switch (_mode) { // case GPS_DRIVER_MODE_UBX: // mode_str = "UBX"; // break; // // case GPS_DRIVER_MODE_MTK: // mode_str = "MTK"; // break; // // case GPS_DRIVER_MODE_ASHTECH: // mode_str = "ASHTECH"; // break; // // default: // break; // } // // PX4_WARN("module found: %s", mode_str); _healthy = true; } } if (_healthy) { PX4_WARN("GPS module lost"); _healthy = false; _rate = 0.0f; _rate_rtcm_injection = 0.0f; } } if (_mode_auto) { switch (_mode) { case GPS_DRIVER_MODE_UBX: _mode = GPS_DRIVER_MODE_MTK; break; case GPS_DRIVER_MODE_MTK: _mode = GPS_DRIVER_MODE_ASHTECH; break; case GPS_DRIVER_MODE_ASHTECH: _mode = GPS_DRIVER_MODE_UBX; break; default: break; } } } } PX4_INFO("exiting"); orb_unsubscribe(_orb_inject_data_fd); if (_dump_communication_pub) { orb_unadvertise(_dump_communication_pub); } ::close(_serial_fd); orb_unadvertise(_report_gps_pos_pub); /* tell the dtor that we are exiting */ _task = -1; px4_task_exit(0); }
PublicationBase::~PublicationBase() { orb_unadvertise(getHandle()); }
void Navigator::task_main() { bool have_geofence_position_data = false; /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file * else clear geofence data in datamanager */ struct stat buffer; if (stat(GEOFENCE_FILENAME, &buffer) == 0) { PX4_INFO("Try to load geofence.txt"); _geofence.loadFromFile(GEOFENCE_FILENAME); } else { if (_geofence.clearDm() != OK) { mavlink_log_critical(&_mavlink_log_pub, "failed clearing geofence"); } } /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); _vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)); /* copy all topics first time */ vehicle_status_update(); vehicle_land_detected_update(); global_position_update(); local_position_update(); gps_position_update(); sensor_combined_update(); home_position_update(true); fw_pos_ctrl_status_update(); params_update(); /* wakeup source(s) */ px4_pollfd_struct_t fds[1] = {}; /* Setup of loop */ fds[0].fd = _global_pos_sub; fds[0].events = POLLIN; bool global_pos_available_once = false; /* rate-limit global pos subscription to 20 Hz / 50 ms */ orb_set_interval(_global_pos_sub, 49); while (!_task_should_exit) { /* wait for up to 1000ms for data */ int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); if (pret == 0) { /* timed out - periodic check for _task_should_exit, etc. */ if (global_pos_available_once) { global_pos_available_once = false; PX4_WARN("global position timeout"); } /* Let the loop run anyway, don't do `continue` here. */ } else if (pret < 0) { /* this is undesirable but not much we can do - might want to flag unhappy status */ PX4_ERR("nav: poll error %d, %d", pret, errno); usleep(10000); continue; } else { if (fds[0].revents & POLLIN) { /* success, global pos is available */ global_position_update(); if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { have_geofence_position_data = true; } global_pos_available_once = true; } } perf_begin(_loop_perf); bool updated; /* gps updated */ orb_check(_gps_pos_sub, &updated); if (updated) { gps_position_update(); if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) { have_geofence_position_data = true; } } /* local position updated */ orb_check(_local_pos_sub, &updated); if (updated) { local_position_update(); } /* sensors combined updated */ orb_check(_sensor_combined_sub, &updated); if (updated) { sensor_combined_update(); } /* parameters updated */ orb_check(_param_update_sub, &updated); if (updated) { params_update(); } /* vehicle status updated */ orb_check(_vstatus_sub, &updated); if (updated) { vehicle_status_update(); } /* vehicle land detected updated */ orb_check(_land_detected_sub, &updated); if (updated) { vehicle_land_detected_update(); } /* navigation capabilities updated */ orb_check(_fw_pos_ctrl_status_sub, &updated); if (updated) { fw_pos_ctrl_status_update(); } /* home position updated */ orb_check(_home_pos_sub, &updated); if (updated) { home_position_update(); } orb_check(_vehicle_command_sub, &updated); if (updated) { vehicle_command_s cmd = {}; orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) { struct position_setpoint_triplet_s *rep = get_reposition_triplet(); // store current position as previous position and goal as next rep->previous.yaw = get_global_position()->yaw; rep->previous.lat = get_global_position()->lat; rep->previous.lon = get_global_position()->lon; rep->previous.alt = get_global_position()->alt; rep->current.loiter_radius = get_loiter_radius(); rep->current.loiter_direction = 1; rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER; // Go on and check which changes had been requested if (PX4_ISFINITE(cmd.param4)) { rep->current.yaw = cmd.param4; } else { rep->current.yaw = NAN; } if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7; rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7; } else { rep->current.lat = get_global_position()->lat; rep->current.lon = get_global_position()->lon; } if (PX4_ISFINITE(cmd.param7)) { rep->current.alt = cmd.param7; } else { rep->current.alt = get_global_position()->alt; } rep->previous.valid = true; rep->current.valid = true; rep->next.valid = false; } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) { struct position_setpoint_triplet_s *rep = get_takeoff_triplet(); // store current position as previous position and goal as next rep->previous.yaw = get_global_position()->yaw; rep->previous.lat = get_global_position()->lat; rep->previous.lon = get_global_position()->lon; rep->previous.alt = get_global_position()->alt; rep->current.loiter_radius = get_loiter_radius(); rep->current.loiter_direction = 1; rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; if (home_position_valid()) { rep->current.yaw = cmd.param4; rep->previous.valid = true; } else { rep->current.yaw = get_local_position()->yaw; rep->previous.valid = false; } if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7; rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7; } else { // If one of them is non-finite, reset both rep->current.lat = NAN; rep->current.lon = NAN; } rep->current.alt = cmd.param7; rep->current.valid = true; rep->next.valid = false; } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_LAND_START) { /* find NAV_CMD_DO_LAND_START in the mission and * use MAV_CMD_MISSION_START to start the mission there */ int land_start = _mission.find_offboard_land_start(); if (land_start != -1) { vehicle_command_s vcmd = {}; vcmd.target_system = get_vstatus()->system_id; vcmd.target_component = get_vstatus()->component_id; vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START; vcmd.param1 = land_start; vcmd.param2 = 0; publish_vehicle_cmd(vcmd); } else { PX4_WARN("planned landing not available"); } } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH) { vehicle_command_s vcmd = {}; vcmd.target_system = get_vstatus()->system_id; vcmd.target_component = get_vstatus()->component_id; vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH; orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH); (void)orb_unadvertise(pub); } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) { if (get_mission_result()->valid && PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0) && (cmd.param1 < _mission_result.seq_total)) { _mission.set_current_offboard_mission_index(cmd.param1); } } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_PAUSE_CONTINUE) { warnx("navigator: got pause/continue command"); } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) { if (cmd.param2 > FLT_EPSILON) { // XXX not differentiating ground and airspeed yet set_cruising_speed(cmd.param2); } else { set_cruising_speed(); /* if no speed target was given try to set throttle */ if (cmd.param3 > FLT_EPSILON) { set_cruising_throttle(cmd.param3 / 100); } else { set_cruising_throttle(); } } } } /* Check geofence violation */ static hrt_abstime last_geofence_check = 0; if (have_geofence_position_data && (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && (hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) { bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, home_position_valid()); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; _geofence_result.timestamp = hrt_absolute_time(); _geofence_result.geofence_action = _geofence.getGeofenceAction(); if (!inside) { /* inform other apps via the mission result */ _geofence_result.geofence_violated = true; /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { mavlink_log_critical(&_mavlink_log_pub, "Geofence violation"); _geofence_violation_warning_sent = true; } } else { /* inform other apps via the mission result */ _geofence_result.geofence_violated = false; /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } publish_geofence_result(); } /* Do stuff according to navigation state set by commander */ switch (_vstatus.nav_state) { case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_mission; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_loiter; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_rcLoss; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_rtl; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_takeoff; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_land; break; case vehicle_status_s::NAVIGATION_STATE_DESCEND: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_land; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_dataLinkLoss; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_engineFailure; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_gpsFailure; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = &_follow_target; break; case vehicle_status_s::NAVIGATION_STATE_MANUAL: case vehicle_status_s::NAVIGATION_STATE_ACRO: case vehicle_status_s::NAVIGATION_STATE_ALTCTL: case vehicle_status_s::NAVIGATION_STATE_POSCTL: case vehicle_status_s::NAVIGATION_STATE_TERMINATION: case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: case vehicle_status_s::NAVIGATION_STATE_STAB: default: _pos_sp_triplet_published_invalid_once = false; _navigation_mode = nullptr; _can_loiter_at_sp = false; break; } /* iterate through navigation modes and set active/inactive for each */ for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { _navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]); } /* if nothing is running, set position setpoint triplet invalid once */ if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) { _pos_sp_triplet_published_invalid_once = true; _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; _pos_sp_triplet_updated = true; } if (_pos_sp_triplet_updated) { _pos_sp_triplet.timestamp = hrt_absolute_time(); publish_position_setpoint_triplet(); _pos_sp_triplet_updated = false; } if (_mission_result_updated) { publish_mission_result(); _mission_result_updated = false; } perf_end(_loop_perf); } PX4_INFO("exiting"); _navigator_task = -1; }
PX4Magnetometer::~PX4Magnetometer() { if (_topic != nullptr) { orb_unadvertise(_topic); } }
void *multirotor_position_control_thread_main() { /* welcome user */ fprintf (stdout, "Multirotor position controller started\n"); fflush(stdout); int i; bool_t updated; bool_t reset_mission_sp = 0 /* false */; bool_t global_pos_sp_valid = 0 /* false */; bool_t reset_man_sp_z = 1 /* true */; bool_t reset_man_sp_xy = 1 /* true */; bool_t reset_int_z = 1 /* true */; bool_t reset_int_z_manual = 0 /* false */; bool_t reset_int_xy = 1 /* true */; bool_t was_armed = 0 /* false */; bool_t reset_auto_sp_xy = 1 /* true */; bool_t reset_auto_sp_z = 1 /* true */; bool_t reset_takeoff_sp = 1 /* true */; absolute_time t_prev = 0; const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; float i_limit; /* use integral_limit_out = tilt_max / 2 */ float ref_alt = 0.0f; absolute_time ref_alt_t = 0; absolute_time local_ref_timestamp = 0; PID_t xy_pos_pids[2]; PID_t xy_vel_pids[2]; PID_t z_pos_pid; thrust_pid_t z_vel_pid; float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; /* structures */ struct parameter_update_s ps; struct vehicle_control_flags_s control_flags; memset(&control_flags, 0, sizeof(control_flags)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; memset(&att_sp, 0, sizeof(att_sp)); struct manual_control_setpoint_s manual; memset(&manual, 0, sizeof(manual)); struct vehicle_local_position_s local_pos; memset(&local_pos, 0, sizeof(local_pos)); struct vehicle_local_position_setpoint_s local_pos_sp; memset(&local_pos_sp, 0, sizeof(local_pos_sp)); struct vehicle_global_position_setpoint_s global_pos_sp; memset(&global_pos_sp, 0, sizeof(global_pos_sp)); struct vehicle_global_velocity_setpoint_s global_vel_sp; memset(&global_vel_sp, 0, sizeof(global_vel_sp)); /* subscribe to attitude, motor setpoints and system state */ orb_subscr_t param_sub = orb_subscribe(ORB_ID(parameter_update)); if (param_sub < 0) { fprintf (stderr, "Position controller thread failed to subscribe to parameter_update topic\n"); exit(-1); } orb_subscr_t control_flags_sub = orb_subscribe(ORB_ID(vehicle_control_flags)); if (control_flags_sub < 0) { fprintf (stderr, "Position controller thread failed to subscribe to vehicle_control_flags topic\n"); exit(-1); } orb_subscr_t att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); if (att_sub < 0) { fprintf (stderr, "Position controller thread failed to subscribe to vehicle_attitude topic\n"); exit(-1); } orb_subscr_t att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); if (att_sp_sub < 0) { fprintf (stderr, "Position controller thread failed to subscribe to vehicle_attitude_setpoint topic\n"); exit(-1); } orb_subscr_t manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); if (manual_sub < 0) { fprintf (stderr, "Position controller thread failed to subscribe to manual_control_setpoint topic\n"); exit(-1); } orb_subscr_t local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); if (local_pos_sp_sub < 0) { fprintf (stderr, "Position controller thread failed to subscribe to vehicle_local_position_setpoint topic\n"); exit(-1); } orb_subscr_t local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); if (local_pos_sub < 0) { fprintf (stderr, "Position controller thread failed to subscribe to vehicle_local_position topic\n"); exit(-1); } orb_subscr_t global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); if (global_pos_sp_sub < 0) { fprintf (stderr, "Position controller thread failed to subscribe to vehicle_global_position_setpoint topic\n"); exit(-1); } /* publish setpoint */ orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint)); if (local_pos_sp_pub == -1) { fprintf (stderr, "Comunicator thread failed to advertise the vehicle_local_position_setpoint topic\n"); exit (-1); } orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint)); if (global_vel_sp_pub == -1) { fprintf (stderr, "Comunicator thread failed to advertise the vehicle_global_velocity_setpoint topic\n"); exit (-1); } orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint)); if (att_sp_pub == -1) { fprintf (stderr, "Comunicator thread failed to advertise the vehicle_attitude_setpoint topic\n"); exit (-1); } orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); /* abort on a nonzero return value from the parameter init */ if (multirotor_position_control_params_init() != 0) { /* parameter setup went wrong, abort */ fprintf (stderr, "Multirotor position controller aborting on startup due to an error\n"); exit(-1); } for (i = 0; i < 2; i++) { pid_init(&(xy_pos_pids[i]), multirotor_position_control_parameters.xy_p, 0.0f, multirotor_position_control_parameters.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); pid_init(&(xy_vel_pids[i]), multirotor_position_control_parameters.xy_vel_p, multirotor_position_control_parameters.xy_vel_i, multirotor_position_control_parameters.xy_vel_d, 1.0f, multirotor_position_control_parameters.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); } pid_init(&z_pos_pid, multirotor_position_control_parameters.z_p, 0.0f, multirotor_position_control_parameters.z_d, 1.0f, multirotor_position_control_parameters.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f); thrust_pid_init(&z_vel_pid, multirotor_position_control_parameters.z_vel_p, multirotor_position_control_parameters.z_vel_i, multirotor_position_control_parameters.z_vel_d, -multirotor_position_control_parameters.thr_max, -multirotor_position_control_parameters.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); while (!_shutdown_all_systems) { updated = orb_check (ORB_ID(parameter_update), param_sub); if (updated) { /* clear updated flag */ orb_copy(ORB_ID(parameter_update), param_sub, &ps); /* update multirotor_position_control_parameters */ multirotor_position_control_params_update(); for (i = 0; i < 2; i++) { pid_set_parameters(&(xy_pos_pids[i]), multirotor_position_control_parameters.xy_p, 0.0f, multirotor_position_control_parameters.xy_d, 1.0f, 0.0f); if (multirotor_position_control_parameters.xy_vel_i > 0.0f) { i_limit = multirotor_position_control_parameters.tilt_max / multirotor_position_control_parameters.xy_vel_i / 2.0f; } else { i_limit = 0.0f; // not used } pid_set_parameters(&(xy_vel_pids[i]), multirotor_position_control_parameters.xy_vel_p, multirotor_position_control_parameters.xy_vel_i, multirotor_position_control_parameters.xy_vel_d, i_limit, multirotor_position_control_parameters.tilt_max); } pid_set_parameters(&z_pos_pid, multirotor_position_control_parameters.z_p, 0.0f, multirotor_position_control_parameters.z_d, 1.0f, multirotor_position_control_parameters.z_vel_max); thrust_pid_set_parameters(&z_vel_pid, multirotor_position_control_parameters.z_vel_p, multirotor_position_control_parameters.z_vel_i, multirotor_position_control_parameters.z_vel_d, -multirotor_position_control_parameters.thr_max, -multirotor_position_control_parameters.thr_min); } updated = orb_check (ORB_ID(vehicle_control_flags), control_flags_sub); if (updated) { orb_copy(ORB_ID(vehicle_control_flags), control_flags_sub, &control_flags); } updated = orb_check (ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub); if (updated) { orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp); global_pos_sp_valid = 1 /* true */; reset_mission_sp = 1 /* true */; } absolute_time t = get_absolute_time(); float dt; if (t_prev != 0) { dt = (t - t_prev) * 0.000001f; } else { dt = 0.0f; } if (control_flags.flag_armed && !was_armed) { /* reset setpoints and integrals on arming */ reset_man_sp_z = 1 /* true */; reset_man_sp_xy = 1 /* true */; reset_auto_sp_z = 1 /* true */; reset_auto_sp_xy = 1 /* true */; reset_takeoff_sp = 1 /* true */; reset_int_z = 1 /* true */; reset_int_xy = 1 /* true */; } was_armed = control_flags.flag_armed; t_prev = t; if (control_flags.flag_control_altitude_enabled || control_flags.flag_control_velocity_enabled || control_flags.flag_control_position_enabled) { orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); float z_sp_offs_max = multirotor_position_control_parameters.z_vel_max / multirotor_position_control_parameters.z_p * 2.0f; float xy_sp_offs_max = multirotor_position_control_parameters.xy_vel_max / multirotor_position_control_parameters.xy_p * 2.0f; float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f }; if (control_flags.flag_control_manual_enabled) { /* manual control */ /* check for reference point updates and correct setpoint */ if (local_pos.ref_timestamp != ref_alt_t) { if (ref_alt_t != 0) { /* home alt changed, don't follow large ground level changes in manual flight */ local_pos_sp.z += local_pos.ref_alt - ref_alt; } ref_alt_t = local_pos.ref_timestamp; ref_alt = local_pos.ref_alt; // TODO also correct XY setpoint } /* reset setpoints to current position if needed */ if (control_flags.flag_control_altitude_enabled) { if (reset_man_sp_z) { reset_man_sp_z = 0 /* false */; local_pos_sp.z = local_pos.z; //mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z); } /* move altitude setpoint with throttle stick */ float z_sp_ctl = scale_control(manual.thrust - 0.5f, 0.5f, alt_ctl_dz); if (z_sp_ctl != 0.0f) { sp_move_rate[2] = -z_sp_ctl * multirotor_position_control_parameters.z_vel_max; local_pos_sp.z += sp_move_rate[2] * dt; if (local_pos_sp.z > local_pos.z + z_sp_offs_max) { local_pos_sp.z = local_pos.z + z_sp_offs_max; } else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) { local_pos_sp.z = local_pos.z - z_sp_offs_max; } } } if (control_flags.flag_control_position_enabled) { if (reset_man_sp_xy) { reset_man_sp_xy = 0 /* false */; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; pid_reset_integral(&xy_vel_pids[0]); pid_reset_integral(&xy_vel_pids[1]); //mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); } /* move position setpoint with roll/pitch stick */ float pos_pitch_sp_ctl = scale_control(-manual.pitch / multirotor_position_control_parameters.rc_scale_pitch, 1.0f, pos_ctl_dz); float pos_roll_sp_ctl = scale_control(manual.roll / multirotor_position_control_parameters.rc_scale_roll, 1.0f, pos_ctl_dz); if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) { /* calculate direction and increment of control in NED frame */ float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * multirotor_position_control_parameters.xy_vel_max; sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed; sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed; local_pos_sp.x += sp_move_rate[0] * dt; local_pos_sp.y += sp_move_rate[1] * dt; /* limit maximum setpoint from position offset and preserve direction * fail safe, should not happen in normal operation */ float pos_vec_x = local_pos_sp.x - local_pos.x; float pos_vec_y = local_pos_sp.y - local_pos.y; float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max; if (pos_vec_norm > 1.0f) { local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm; local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm; } } } /* copy yaw setpoint to vehicle_local_position_setpoint topic */ local_pos_sp.yaw = att_sp.yaw_body; /* local position setpoint is valid and can be used for auto loiter after position controlled mode */ reset_auto_sp_xy = !control_flags.flag_control_position_enabled; reset_auto_sp_z = !control_flags.flag_control_altitude_enabled; reset_takeoff_sp = 1 /* true */; /* force reprojection of global setpoint after manual mode */ reset_mission_sp = 1 /* true */; } else if (control_flags.flag_control_auto_enabled) { /* AUTO mode, use global setpoint */ if (control_flags.auto_state == navigation_state_auto_ready) { reset_auto_sp_xy = 1 /* true */; reset_auto_sp_z = 1 /* true */; } else if (control_flags.auto_state == navigation_state_auto_takeoff) { if (reset_takeoff_sp) { reset_takeoff_sp = 0 /* false */; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; local_pos_sp.z = - multirotor_position_control_parameters.takeoff_alt - multirotor_position_control_parameters.takeoff_gap; att_sp.yaw_body = att.yaw; //mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z); } reset_auto_sp_xy = 0 /* false */; reset_auto_sp_z = 1 /* true */; } else if (control_flags.auto_state == navigation_state_auto_rtl) { // TODO reset_auto_sp_xy = 1 /* true */; reset_auto_sp_z = 1 /* true */; } else if (control_flags.auto_state == navigation_state_auto_mission) { /* init local projection using local position ref */ if (local_pos.ref_timestamp != local_ref_timestamp) { reset_mission_sp = 1 /* true */; local_ref_timestamp = local_pos.ref_timestamp; double lat_home = local_pos.ref_lat * 1e-7; double lon_home = local_pos.ref_lon * 1e-7; map_projection_init(lat_home, lon_home); //mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home); } if (reset_mission_sp) { reset_mission_sp = 0 /* false */; /* update global setpoint projection */ if (global_pos_sp_valid) { /* global position setpoint valid, use it */ double sp_lat = global_pos_sp.latitude * 1e-7; double sp_lon = global_pos_sp.longitude * 1e-7; /* project global setpoint to local setpoint */ map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); if (global_pos_sp.altitude_is_relative) { local_pos_sp.z = -global_pos_sp.altitude; } else { local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; } att_sp.yaw_body = global_pos_sp.yaw; //mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y); } else { if (reset_auto_sp_xy) { reset_auto_sp_xy = 0 /* false */; /* local position setpoint is invalid, * use current position as setpoint for loiter */ local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; local_pos_sp.yaw = att.yaw; } if (reset_auto_sp_z) { reset_auto_sp_z = 0 /* false */; local_pos_sp.z = local_pos.z; } //mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); } } reset_auto_sp_xy = 1 /* true */; reset_auto_sp_z = 1 /* true */; } if (control_flags.auto_state != navigation_state_auto_takeoff) { reset_takeoff_sp = 1 /* true */; } if (control_flags.auto_state != navigation_state_auto_mission) { reset_mission_sp = 1 /* true */; } /* copy yaw setpoint to vehicle_local_position_setpoint topic */ local_pos_sp.yaw = att_sp.yaw_body; /* reset setpoints after AUTO mode */ reset_man_sp_xy = 1 /* true */; reset_man_sp_z = 1 /* true */; } else { /* no control (failsafe), loiter or stay on ground */ if (local_pos.landed) { /* landed: move setpoint down */ /* in air: hold altitude */ if (local_pos_sp.z < 5.0f) { /* set altitude setpoint to 5m under ground, * don't set it too deep to avoid unexpected landing in case of false "landed" signal */ local_pos_sp.z = 5.0f; //mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z); } reset_man_sp_z = 1 /* true */; } else { /* in air: hold altitude */ if (reset_man_sp_z) { reset_man_sp_z = 0 /* false */; local_pos_sp.z = local_pos.z; //mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z); } reset_auto_sp_z = 0 /* false */; } if (control_flags.flag_control_position_enabled) { if (reset_man_sp_xy) { reset_man_sp_xy = 0 /* false */; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; local_pos_sp.yaw = att.yaw; att_sp.yaw_body = att.yaw; //mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); } reset_auto_sp_xy = 0 /* false */; } } /* publish local position setpoint */ orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); /* run position & altitude controllers, calculate velocity setpoint */ if (control_flags.flag_control_altitude_enabled) { global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2]; } else { reset_man_sp_z = 1 /* true */; global_vel_sp.vz = 0.0f; } if (control_flags.flag_control_position_enabled) { /* calculate velocity set point in NED frame */ global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0]; global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1]; /* limit horizontal speed */ float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / multirotor_position_control_parameters.xy_vel_max; if (xy_vel_sp_norm > 1.0f) { global_vel_sp.vx /= xy_vel_sp_norm; global_vel_sp.vy /= xy_vel_sp_norm; } } else { reset_man_sp_xy = 1 /* true */; global_vel_sp.vx = 0.0f; global_vel_sp.vy = 0.0f; } //fprintf (stderr, "global_vel_sp.vx:%.3f\tglobal_vel_sp.vy:%.3f\tglobal_vel_sp.vz:%.3f\n", global_vel_sp.vx, global_vel_sp.vy, global_vel_sp.vz); /* publish new velocity setpoint */ orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); // TODO subscribe to velocity setpoint if altitude/position control disabled if (control_flags.flag_control_climb_rate_enabled || control_flags.flag_control_velocity_enabled) { /* run velocity controllers, calculate thrust vector with attitude-thrust compensation */ if (control_flags.flag_control_climb_rate_enabled) { if (reset_int_z) { reset_int_z = 0 /* false */; float i = multirotor_position_control_parameters.thr_min; if (reset_int_z_manual) { i = manual.thrust; if (i < multirotor_position_control_parameters.thr_min) { i = multirotor_position_control_parameters.thr_min; } else if (i > multirotor_position_control_parameters.thr_max) { i = multirotor_position_control_parameters.thr_max; } } thrust_pid_set_integral(&z_vel_pid, -i); //mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); } thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); att_sp.thrust = -thrust_sp[2]; } else { /* reset thrust integral when altitude control enabled */ reset_int_z = 1 /* true */; } if (control_flags.flag_control_velocity_enabled) { /* calculate thrust set point in NED frame */ if (reset_int_xy) { reset_int_xy = 0 /* false */; pid_reset_integral(&xy_vel_pids[0]); pid_reset_integral(&xy_vel_pids[1]); //mavlink_log_info(mavlink_fd, "[mpc] reset pos integral"); } thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt); thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt); /* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */ /* limit horizontal part of thrust */ float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]); /* assuming that vertical component of thrust is g, * horizontal component = g * tan(alpha) */ float tilt = atanf(norm(thrust_sp[0], thrust_sp[1])); if (tilt > multirotor_position_control_parameters.tilt_max) { tilt = multirotor_position_control_parameters.tilt_max; } /* convert direction to body frame */ thrust_xy_dir -= att.yaw; /* calculate roll and pitch */ att_sp.roll_body = sinf(thrust_xy_dir) * tilt; att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body); } else { reset_int_xy = 1 /* true */; } att_sp.timestamp = get_absolute_time(); //fprintf (stderr, "att_sp.roll:%.3f\tatt_sp.pitch:%.3f\tatt_sp.yaw:%.3f\tatt_sp.thrust:%.3f\n", att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, att_sp.thrust); /* publish new attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } } else { /* position controller disabled, reset setpoints */ reset_man_sp_z = 1 /* true */; reset_man_sp_xy = 1 /* true */; reset_int_z = 1 /* true */; reset_int_xy = 1 /* true */; reset_mission_sp = 1 /* true */; reset_auto_sp_xy = 1 /* true */; reset_auto_sp_z = 1 /* true */; } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ reset_int_z_manual = control_flags.flag_armed && control_flags.flag_control_manual_enabled && !control_flags.flag_control_climb_rate_enabled; /* run at approximately 50 Hz */ usleep(20000); } /* * do unsubscriptions */ orb_unsubscribe(ORB_ID(parameter_update), param_sub, pthread_self()); orb_unsubscribe(ORB_ID(vehicle_control_flags), control_flags_sub, pthread_self()); orb_unsubscribe(ORB_ID(vehicle_attitude), att_sub, pthread_self()); orb_unsubscribe(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, pthread_self()); orb_unsubscribe(ORB_ID(manual_control_setpoint), manual_sub, pthread_self()); orb_unsubscribe(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, pthread_self()); orb_unsubscribe(ORB_ID(vehicle_local_position), local_pos_sub, pthread_self()); orb_unsubscribe(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, pthread_self()); /* * do unadvertises */ orb_unadvertise(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, pthread_self()); orb_unadvertise(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, pthread_self()); orb_unadvertise(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, pthread_self()); return 0; }