void uORB::KraitFastRpcChannel::fastrpc_recv_thread()
{
// sit in while loop.
int32_t rc = 0;
int32_t data_length = 0;
uint8_t *data = nullptr;
unsigned long rpc_min, rpc_max;
unsigned long orb_min, orb_max;
double rpc_avg, orb_avg;
unsigned long count = 0;
rpc_max = orb_max = 0;
rpc_min = orb_min = 0xFFFFFFFF;
rpc_avg = orb_avg = 0.0;
int32_t num_topics = 0;
hrt_abstime check_time = 0;
while (!_ThreadShouldExit) {
hrt_abstime t1, t2, t3;
t1 = hrt_absolute_time();
rc = _KraitWrapper.ReceiveBulkData(&data, &data_length, &num_topics);
t2 = hrt_absolute_time();
if (rc == 0) {
//PX4_DEBUG( "Num of topics Received: %d", num_topics );
int32_t bytes_processed = 0;
for (int i = 0; i < num_topics; ++i) {
uint8_t *new_pkt = &(data[bytes_processed]);
struct BulkTransferHeader *header = (struct BulkTransferHeader *)new_pkt;
char *messageName = (char *)(new_pkt + sizeof(struct BulkTransferHeader));
uint16_t check_msg_len = strlen(messageName);
if (header->_MsgNameLen != (check_msg_len + 1)) {
PX4_ERR("Error: Packing error. Sent Msg Len. of[%d] but strlen returned:[%d]", header->_MsgNameLen , check_msg_len);
PX4_ERR("Error: NumTopics: %d processing topic: %d msgLen[%d] dataLen[%d] data_len[%d] bytes processed: %d",
num_topics, i, header->_MsgNameLen, header->_DataLen , data_length, bytes_processed);
DumpData(data, data_length, num_topics);
break;
}
uint8_t *topic_data = (uint8_t *)(messageName + strlen(messageName) + 1);
if (_RxHandler != nullptr) {
if (header->_MsgType == _DATA_MSG_TYPE) {
//PX4_DEBUG( "Received topic data for: [%s] len[%d]\n", messageName, data_length );
_RxHandler->process_received_message(messageName,
header->_DataLen, topic_data);
} else if (header->_MsgType == _CONTROL_MSG_TYPE_ADVERTISE) {
PX4_DEBUG("Received topic advertise message for: [%s] len[%d]\n", messageName, data_length);
_RxHandler->process_remote_topic(messageName, true);
} else if (header->_MsgType == _CONTROL_MSG_TYPE_UNADVERTISE) {
PX4_DEBUG("Received topic unadvertise message for: [%s] len[%d]\n", messageName, data_length);
_RxHandler->process_remote_topic(messageName, false);
}
}
bytes_processed += header->_MsgNameLen + header->_DataLen + sizeof(struct BulkTransferHeader);
}
} else {
PX4_DEBUG("Error: Getting data over fastRPC channel\n");
break;
}
t3 = hrt_absolute_time();
count++;
if ((unsigned long)(t2 - t1) < rpc_min) {
rpc_min = (unsigned long)(t2 - t1);
}
if ((unsigned long)(t2 - t1) > rpc_max) {
rpc_max = (unsigned long)(t2 - t1);
}
if ((unsigned long)(t3 - t2) < orb_min) {
orb_min = (unsigned long)(t3 - t2);
}
if ((unsigned long)(t3 - t2) > orb_max) {
orb_max = (unsigned long)(t3 - t2);
}
rpc_avg = ((double)((rpc_avg * (count - 1)) + (unsigned long)(t2 - t1))) / (double)(count);
orb_avg = ((double)((orb_avg * (count - 1)) + (unsigned long)(t3 - t2))) / (double)(count);
if ((unsigned long)(t3 - check_time) >= 10000000) {
//PX4_DEBUG("Krait RPC Stats : rpc_min: %lu rpc_max: %lu rpc_avg: %f", rpc_min, rpc_max, rpc_avg);
//PX4_DEBUG("Krait RPC(orb) Stats: orb_min: %lu orb_max: %lu orb_avg: %f", orb_min, orb_max, orb_avg);
check_time = t3;
rpc_max = orb_max = 0;
rpc_min = orb_min = 0xFFFFFF;
orb_avg = 0;
rpc_avg = 0;
count = 0;
}
//PX4_DEBUG("MsgName: %30s, t1: %lu, t2: %lu, t3: %lu, dt1: %lu, dt2: %lu",name, (unsigned long) t1, (unsigned long) t2, (unsigned long) t3,
// (unsigned long) (t2-t1), (unsigned long) (t3-t2));
}
PX4_DEBUG("[uORB::KraitFastRpcChannel::fastrpc_recv_thread] Exiting fastrpc_recv_thread\n");
}
int do_mag_calibration(orb_advert_t *mavlink_log_pub)
{
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
struct mag_calibration_s mscale_null;
mscale_null.x_offset = 0.0f;
mscale_null.x_scale = 1.0f;
mscale_null.y_offset = 0.0f;
mscale_null.y_scale = 1.0f;
mscale_null.z_offset = 0.0f;
mscale_null.z_scale = 1.0f;
int result = OK;
// Determine which mags are available and reset each
char str[30];
for (size_t i=0; i<max_mags; i++) {
device_ids[i] = 0; // signals no mag
}
for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
#ifndef __PX4_QURT
// Reset mag id to mag not available
(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
result = param_set_no_notification(param_find(str), &(device_ids[cur_mag]));
if (result != OK) {
calibration_log_info(mavlink_log_pub, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag);
break;
}
#else
(void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag);
result = param_set(param_find(str), &mscale_null.x_offset);
if (result != OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag);
result = param_set(param_find(str), &mscale_null.y_offset);
if (result != OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag);
result = param_set(param_find(str), &mscale_null.z_offset);
if (result != OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag);
result = param_set(param_find(str), &mscale_null.x_scale);
if (result != OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
result = param_set(param_find(str), &mscale_null.y_scale);
if (result != OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag);
result = param_set(param_find(str), &mscale_null.z_scale);
if (result != OK) {
PX4_ERR("unable to reset %s", str);
}
#endif
/* for calibration, commander will run on apps, so orb messages are used to get info from dsp */
#ifndef __PX4_QURT
// Attempt to open mag
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
int fd = px4_open(str, O_RDONLY);
if (fd < 0) {
continue;
}
// Get device id for this mag
device_ids[cur_mag] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
internal[cur_mag] = (px4_ioctl(fd, MAGIOCGEXTERNAL, 0) <= 0);
// Reset mag scale
result = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
if (result != OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, cur_mag);
}
/* calibrate range */
if (result == OK) {
result = px4_ioctl(fd, MAGIOCCALIBRATE, fd);
if (result != OK) {
calibration_log_info(mavlink_log_pub, "[cal] Skipped scale calibration, sensor %u", cur_mag);
/* this is non-fatal - mark it accordingly */
result = OK;
}
}
px4_close(fd);
#endif
}
// Calibrate all mags at the same time
if (result == OK) {
switch (mag_calibrate_all(mavlink_log_pub, device_ids)) {
case calibrate_return_cancelled:
// Cancel message already displayed, we're done here
result = ERROR;
break;
case calibrate_return_ok:
/* auto-save to EEPROM */
result = param_save_default();
/* if there is a any preflight-check system response, let the barrage of messages through */
usleep(200000);
if (result == OK) {
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
break;
} else {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SAVE_PARAMS_MSG);
}
// Fall through
default:
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
break;
}
}
/* give this message enough time to propagate */
usleep(600000);
return result;
}
/**
* @brief Performs some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
void
test()
{
struct distance_sensor_s report;
ssize_t sz;
int ret;
if (!instance) {
PX4_ERR("No ll40ls driver running");
return;
}
int fd = px4_open(instance->get_dev_name(), O_RDONLY);
if (fd < 0) {
PX4_ERR("Error opening fd");
return;
}
/* Do a simple demand read. */
sz = px4_read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
PX4_ERR("immediate read failed");
return;
}
print_message(report);
/* Start the sensor polling at 2Hz. */
if (PX4_OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
PX4_ERR("failed to set 2Hz poll rate");
return;
}
/* Read the sensor 5 times and report each value. */
for (unsigned i = 0; i < 5; i++) {
px4_pollfd_struct_t fds;
/* Wait for data to be ready. */
fds.fd = fd;
fds.events = POLLIN;
ret = px4_poll(&fds, 1, 2000);
if (ret != 1) {
PX4_WARN("timed out waiting for sensor data");
return;
}
/* Now go get it. */
sz = px4_read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
PX4_WARN("periodic read failed");
return;
}
print_message(report);
}
/* Reset the sensor polling to default rate. */
if (PX4_OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
PX4_WARN("failed to set default poll rate");
}
px4_close(fd);
}
void task_main(int argc, char *argv[])
{
PX4_INFO("enter uart_esc task_main");
_outputs_pub = nullptr;
parameters_init();
esc = UartEsc::get_instance();
if (esc == NULL) {
PX4_ERR("failed to new UartEsc instance");
} else if (esc->initialize((enum esc_model_t)_parameters.model,
_device, _parameters.baudrate) < 0) {
PX4_ERR("failed to initialize UartEsc");
} else {
// Subscribe for orb topics
_controls_sub = orb_subscribe(ORB_ID(actuator_controls_0)); // single group for now
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_param_sub = orb_subscribe(ORB_ID(parameter_update));
// initialize publication structures
memset(&_outputs, 0, sizeof(_outputs));
// set up poll topic and limit poll interval
px4_pollfd_struct_t fds[1];
fds[0].fd = _controls_sub;
fds[0].events = POLLIN;
//orb_set_interval(_controls_sub, 10); // max actuator update period, ms
// set up mixer
if (initialize_mixer(MIXER_FILENAME) < 0) {
PX4_ERR("Mixer initialization failed.");
_task_should_exit = true;
}
// Main loop
while (!_task_should_exit) {
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit */
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
PX4_WARN("poll error %d, %d", pret, errno);
/* sleep a bit before next try */
usleep(100000);
continue;
}
// Handle new actuator controls data
if (fds[0].revents & POLLIN) {
// Grab new controls data
orb_copy(ORB_ID(actuator_controls_0), _controls_sub, &_controls);
// Mix to the outputs
_outputs.timestamp = hrt_absolute_time();
int16_t motor_rpms[UART_ESC_MAX_MOTORS];
if (_armed.armed) {
_outputs.noutputs = mixer->mix(&_outputs.output[0],
actuator_controls_s::NUM_ACTUATOR_CONTROLS,
NULL);
// Make sure we support only up to UART_ESC_MAX_MOTORS motors
if (_outputs.noutputs > UART_ESC_MAX_MOTORS) {
PX4_ERR("Unsupported motors %d, up to %d motors supported",
_outputs.noutputs, UART_ESC_MAX_MOTORS);
continue;
}
// Send outputs to the ESCs
for (unsigned outIdx = 0; outIdx < _outputs.noutputs; outIdx++) {
// map -1.0 - 1.0 outputs to RPMs
motor_rpms[outIdx] = (int16_t)(((_outputs.output[outIdx] + 1.0) / 2.0) *
(esc->max_rpm() - esc->min_rpm()) + esc->min_rpm());
}
uart_esc_rotate_motors(motor_rpms, _outputs.noutputs);
} else {
_outputs.noutputs = UART_ESC_MAX_MOTORS;
for (unsigned outIdx = 0; outIdx < _outputs.noutputs; outIdx++) {
motor_rpms[outIdx] = 0;
_outputs.output[outIdx] = -1.0;
}
}
esc->send_rpms(&motor_rpms[0], _outputs.noutputs);
// TODO-JYW: TESTING-TESTING
// MAINTAIN FOR REFERENCE, COMMENT OUT FOR RELEASE BUILDS
// static int count=0;
// count++;
// if (!(count % 100)) {
// PX4_DEBUG(" ");
// PX4_DEBUG("Time t: %13lu, Armed: %d ",(unsigned long)_outputs.timestamp,_armed.armed);
// PX4_DEBUG("Act Controls: 0: %+8.4f, 1: %+8.4f, 2: %+8.4f, 3: %+8.4f ",_controls.control[0],_controls.control[1],_controls.control[2],_controls.control[3]);
// PX4_DEBUG("Act Outputs : 0: %+8.4f, 1: %+8.4f, 2: %+8.4f, 3: %+8.4f ",_outputs.output[0],_outputs.output[1],_outputs.output[2],_outputs.output[3]);
// }
// TODO-JYW: TESTING-TESTING
/* Publish mixed control outputs */
if (_outputs_pub != nullptr) {
orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);
} else {
_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
}
}
// Check for updates in other subscripions
bool updated = false;
orb_check(_armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
PX4_DEBUG("arming status updated, _armed.armed: %d", _armed.armed);
}
orb_check(_param_sub, &updated);
if (updated) {
// Even though we are only interested in the update status of the parameters, copy
// the subscription to clear the update status.
orb_copy(ORB_ID(parameter_update), _param_sub, &_param_update);
parameters_update();
}
}
}
PX4_WARN("closing uart_esc");
delete esc;
}
int
test_mount(int argc, char *argv[])
{
const unsigned iterations = 2000;
const unsigned alignments = 10;
const char *cmd_filename = "/fs/microsd/mount_test_cmds.txt";
/* check if microSD card is mounted */
struct stat buffer;
if (stat(PX4_ROOTFSDIR "/fs/microsd/", &buffer)) {
PX4_ERR("no microSD card mounted, aborting file test");
return 1;
}
/* list directory */
DIR *d;
struct dirent *dir;
d = opendir(PX4_ROOTFSDIR "/fs/microsd");
if (d) {
while ((dir = readdir(d)) != NULL) {
//printf("%s\n", dir->d_name);
}
closedir(d);
PX4_INFO("directory listing ok (FS mounted and readable)");
} else {
/* failed opening dir */
PX4_ERR("FAILED LISTING MICROSD ROOT DIRECTORY");
if (stat(cmd_filename, &buffer) == OK) {
(void)unlink(cmd_filename);
}
return 1;
}
/* read current test status from file, write test instructions for next round */
/* initial values */
int it_left_fsync = fsync_tries;
int it_left_abort = abort_tries;
int cmd_fd;
if (stat(cmd_filename, &buffer) == OK) {
/* command file exists, read off state */
cmd_fd = open(cmd_filename, O_RDWR | O_NONBLOCK);
char buf[64];
int ret = read(cmd_fd, buf, sizeof(buf));
if (ret > 0) {
int count = 0;
ret = sscanf(buf, "TEST: %u %u %n", &it_left_fsync, &it_left_abort, &count);
} else {
buf[0] = '\0';
}
if (it_left_fsync > fsync_tries) {
it_left_fsync = fsync_tries;
}
if (it_left_abort > abort_tries) {
it_left_abort = abort_tries;
}
PX4_INFO("Iterations left: #%d / #%d of %d / %d\n(%s)", it_left_fsync, it_left_abort,
fsync_tries, abort_tries, buf);
int it_left_fsync_prev = it_left_fsync;
/* now write again what to do next */
if (it_left_fsync > 0) {
it_left_fsync--;
}
if (it_left_fsync == 0 && it_left_abort > 0) {
it_left_abort--;
/* announce mode switch */
if (it_left_fsync_prev != it_left_fsync && it_left_fsync == 0) {
PX4_INFO("\n SUCCESSFULLY PASSED FSYNC'ED WRITES, CONTINUTING WITHOUT FSYNC");
fsync(fileno(stdout));
fsync(fileno(stderr));
usleep(20000);
}
}
if (it_left_abort == 0) {
close(cmd_fd);
(void)unlink(cmd_filename);
return 0;
}
} else {
/* this must be the first iteration, do something */
cmd_fd = open(cmd_filename, O_TRUNC | O_WRONLY | O_CREAT, PX4_O_MODE_666);
PX4_INFO("First iteration of file test\n");
}
char buf[64];
(void)sprintf(buf, "TEST: %d %d ", it_left_fsync, it_left_abort);
lseek(cmd_fd, 0, SEEK_SET);
write(cmd_fd, buf, strlen(buf) + 1);
fsync(cmd_fd);
/* perform tests for a range of chunk sizes */
unsigned chunk_sizes[] = {32, 64, 128, 256, 512, 600, 1200};
for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) {
printf("\n\n====== FILE TEST: %u bytes chunks (%s) ======\n", chunk_sizes[c],
(it_left_fsync > 0) ? "FSYNC" : "NO FSYNC");
printf("unpower the system immediately (within 0.5s) when the hash (#) sign appears\n");
fsync(fileno(stdout));
fsync(fileno(stderr));
usleep(50000);
for (unsigned a = 0; a < alignments; a++) {
printf(".");
uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
/* fill write buffer with known values */
for (unsigned i = 0; i < sizeof(write_buf); i++) {
/* this will wrap, but we just need a known value with spacing */
write_buf[i] = i + 11;
}
uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
int fd = px4_open(PX4_ROOTFSDIR "/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
for (unsigned i = 0; i < iterations; i++) {
int wret = write(fd, write_buf + a, chunk_sizes[c]);
if (wret != (int)chunk_sizes[c]) {
PX4_ERR("WRITE ERROR!");
if ((0x3 & (uintptr_t)(write_buf + a))) {
PX4_ERR("memory is unaligned, align shift: %d", a);
}
return 1;
}
if (it_left_fsync > 0) {
fsync(fd);
} else {
printf("#");
fsync(fileno(stdout));
fsync(fileno(stderr));
}
}
if (it_left_fsync > 0) {
printf("#");
}
printf(".");
fsync(fileno(stdout));
fsync(fileno(stderr));
usleep(200000);
px4_close(fd);
fd = px4_open(PX4_ROOTFSDIR "/fs/microsd/testfile", O_RDONLY);
/* read back data for validation */
for (unsigned i = 0; i < iterations; i++) {
int rret = read(fd, read_buf, chunk_sizes[c]);
if (rret != (int)chunk_sizes[c]) {
PX4_ERR("READ ERROR!");
return 1;
}
/* compare value */
bool compare_ok = true;
for (unsigned j = 0; j < chunk_sizes[c]; j++) {
if (read_buf[j] != write_buf[j + a]) {
PX4_WARN("COMPARISON ERROR: byte %d, align shift: %d", j, a);
compare_ok = false;
break;
}
}
if (!compare_ok) {
PX4_ERR("ABORTING FURTHER COMPARISON DUE TO ERROR");
return 1;
}
}
int ret = unlink(PX4_ROOTFSDIR "/fs/microsd/testfile");
px4_close(fd);
if (ret) {
close(cmd_fd);
PX4_ERR("UNLINKING FILE FAILED");
return 1;
}
}
}
fsync(fileno(stdout));
fsync(fileno(stderr));
usleep(20000);
close(cmd_fd);
/* we always reboot for the next test if we get here */
PX4_INFO("Iteration done, rebooting..");
fsync(fileno(stdout));
fsync(fileno(stderr));
usleep(50000);
px4_systemreset(false);
/* never going to get here */
return 0;
}
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));
_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));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_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();
vehicle_control_mode_update();
global_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;
while (!_task_should_exit) {
/* wait for up to 200ms 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;
}
}
/* 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();
updateParams();
}
/* vehicle control mode updated */
orb_check(_control_mode_sub, &updated);
if (updated) {
vehicle_control_mode_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;
rep->current.yaw = cmd.param4;
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->previous.valid = true;
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
*/
unsigned 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;
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.geofence_action = _geofence.getGeofenceAction();
if (!inside) {
/* inform other apps via the mission result */
_geofence_result.geofence_violated = true;
publish_geofence_result();
/* 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;
publish_geofence_result();
/* Reset the _geofence_violation_warning_sent field */
_geofence_violation_warning_sent = false;
}
}
/* 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;
return;
}
/// @brief Processes an FTP message
void
MavlinkFTP::_process_request(mavlink_file_transfer_protocol_t *ftp_req, uint8_t target_system_id)
{
bool stream_send = false;
PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&ftp_req->payload[0]);
ErrorCode errorCode = kErrNone;
if (!_ensure_buffers_exist()) {
PX4_ERR("Failed to allocate buffers");
errorCode = kErrFailErrno;
errno = ENOMEM;
goto out;
}
// basic sanity checks; must validate length before use
if (payload->size > kMaxDataLength) {
errorCode = kErrInvalidDataSize;
goto out;
}
// check the sequence number: if this is a resent request, resend the last response
if (_last_reply_valid) {
mavlink_file_transfer_protocol_t *last_reply = reinterpret_cast<mavlink_file_transfer_protocol_t *>(_last_reply);
PayloadHeader *last_payload = reinterpret_cast<PayloadHeader *>(&last_reply->payload[0]);
if (payload->seq_number + 1 == last_payload->seq_number) {
// this is the same request as the one we replied to last. It means the (n)ack got lost, and the GCS
// resent the request
mavlink_msg_file_transfer_protocol_send_struct(_mavlink->get_channel(), last_reply);
return;
}
}
#ifdef MAVLINK_FTP_DEBUG
printf("ftp: channel %u opc %u size %u offset %u\n", _getServerChannel(), payload->opcode, payload->size,
payload->offset);
#endif
switch (payload->opcode) {
case kCmdNone:
break;
case kCmdTerminateSession:
errorCode = _workTerminate(payload);
break;
case kCmdResetSessions:
errorCode = _workReset(payload);
break;
case kCmdListDirectory:
errorCode = _workList(payload);
break;
case kCmdOpenFileRO:
errorCode = _workOpen(payload, O_RDONLY);
break;
case kCmdCreateFile:
errorCode = _workOpen(payload, O_CREAT | O_EXCL | O_WRONLY);
break;
case kCmdOpenFileWO:
errorCode = _workOpen(payload, O_CREAT | O_WRONLY);
break;
case kCmdReadFile:
errorCode = _workRead(payload);
break;
case kCmdBurstReadFile:
errorCode = _workBurst(payload, target_system_id);
stream_send = true;
break;
case kCmdWriteFile:
errorCode = _workWrite(payload);
break;
case kCmdRemoveFile:
errorCode = _workRemoveFile(payload);
break;
case kCmdRename:
errorCode = _workRename(payload);
break;
case kCmdTruncateFile:
errorCode = _workTruncateFile(payload);
break;
case kCmdCreateDirectory:
errorCode = _workCreateDirectory(payload);
break;
case kCmdRemoveDirectory:
errorCode = _workRemoveDirectory(payload);
break;
case kCmdCalcFileCRC32:
errorCode = _workCalcFileCRC32(payload);
break;
default:
errorCode = kErrUnknownCommand;
break;
}
out:
payload->seq_number++;
// handle success vs. error
if (errorCode == kErrNone) {
payload->req_opcode = payload->opcode;
payload->opcode = kRspAck;
} else {
int r_errno = errno;
payload->req_opcode = payload->opcode;
payload->opcode = kRspNak;
payload->size = 1;
if (r_errno == EEXIST) {
errorCode = kErrFailFileExists;
}
payload->data[0] = errorCode;
if (errorCode == kErrFailErrno) {
payload->size = 2;
payload->data[1] = r_errno;
}
}
_last_reply_valid = false;
// Stream download replies are sent through mavlink stream mechanism. Unless we need to Nack.
if (!stream_send || errorCode != kErrNone) {
// respond to the request
ftp_req->target_system = target_system_id;
_reply(ftp_req);
}
}
void task_main(int argc, char *argv[])
{
_is_running = true;
if (uart_initialize(_device) < 0) {
PX4_ERR("Failed to initialize UART.");
return;
}
// Subscribe for orb topics
_controls_sub = orb_subscribe(ORB_ID(actuator_controls_0));
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
// Start disarmed
_armed.armed = false;
_armed.prearmed = false;
// Set up poll topic
px4_pollfd_struct_t fds[1];
fds[0].fd = _controls_sub;
fds[0].events = POLLIN;
/* Don't limit poll intervall for now, 250 Hz should be fine. */
//orb_set_interval(_controls_sub, 10);
// Set up mixer
if (initialize_mixer(MIXER_FILENAME) < 0) {
PX4_ERR("Mixer initialization failed.");
return;
}
pwm_limit_init(&_pwm_limit);
// TODO XXX: this is needed otherwise we crash in the callback context.
_rc_pub = orb_advertise(ORB_ID(input_rc), &_rc);
// Main loop
while (!_task_should_exit) {
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 10);
/* Timed out, do a periodic check for _task_should_exit. */
if (pret == 0) {
continue;
}
/* This is undesirable but not much we can do. */
if (pret < 0) {
PX4_WARN("poll error %d, %d", pret, errno);
/* sleep a bit before next try */
usleep(100000);
continue;
}
if (fds[0].revents & POLLIN) {
orb_copy(ORB_ID(actuator_controls_0), _controls_sub, &_controls);
_outputs.timestamp = _controls.timestamp;
/* do mixing */
_outputs.noutputs = _mixer->mix(_outputs.output, 0 /* not used */, NULL);
/* disable unused ports by setting their output to NaN */
for (size_t i = _outputs.noutputs; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {
_outputs.output[i] = NAN;
}
const uint16_t reverse_mask = 0;
uint16_t disarmed_pwm[4];
uint16_t min_pwm[4];
uint16_t max_pwm[4];
for (unsigned int i = 0; i < 4; i++) {
disarmed_pwm[i] = _pwm_disarmed;
min_pwm[i] = _pwm_min;
max_pwm[i] = _pwm_max;
}
uint16_t pwm[4];
// TODO FIXME: pre-armed seems broken
pwm_limit_calc(_armed.armed, false/*_armed.prearmed*/, _outputs.noutputs, reverse_mask,
disarmed_pwm, min_pwm, max_pwm, _outputs.output, pwm, &_pwm_limit);
send_outputs_mavlink(pwm, 4);
if (_outputs_pub != nullptr) {
orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);
} else {
_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
}
}
bool updated;
orb_check(_armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
}
}
uart_deinitialize();
orb_unsubscribe(_controls_sub);
orb_unsubscribe(_armed_sub);
_is_running = false;
}
/**
* Reset the driver.
*/
void
reset()
{
PX4_ERR("GPS reset not supported");
return;
}
int
tfmini_main(int argc, char *argv[])
{
int ch;
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
const char *device_path = "";
int myoptind = 1;
const char *myoptarg = nullptr;
while ((ch = px4_getopt(argc, argv, "R:d:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'R':
rotation = (uint8_t)atoi(myoptarg);
break;
case 'd':
device_path = myoptarg;
break;
default:
PX4_WARN("Unknown option!");
return -1;
}
}
if (myoptind >= argc) {
goto out_error;
}
/*
* Start/load the driver.
*/
if (!strcmp(argv[myoptind], "start")) {
if (strcmp(device_path, "") != 0) {
return tfmini::start(device_path, rotation);
} else {
PX4_WARN("Please specify device path!");
tfmini::usage();
return -1;
}
}
/*
* Stop the driver
*/
if (!strcmp(argv[myoptind], "stop")) {
return tfmini::stop();
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[myoptind], "test")) {
return tfmini::test();
}
/*
* Print driver information.
*/
if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
tfmini::info();
return 0;
}
out_error:
PX4_ERR("unrecognized command, try 'start', 'test', or 'info'");
return -1;
}
px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry,
char *const argv[])
{
int i;
int argc = 0;
unsigned int len = 0;
struct sched_param param = {};
char *p = (char *)argv;
// Calculate argc
while (p != (char *)nullptr) {
p = argv[argc];
if (p == (char *)nullptr) {
break;
}
++argc;
len += strlen(p) + 1;
}
unsigned long structsize = sizeof(pthdata_t) + (argc + 1) * sizeof(char *);
// not safe to pass stack data to the thread creation
pthdata_t *taskdata = (pthdata_t *)malloc(structsize + len);
if (taskdata == nullptr) {
return -ENOMEM;
}
memset(taskdata, 0, structsize + len);
unsigned long offset = ((unsigned long)taskdata) + structsize;
strncpy(taskdata->name, name, 16);
taskdata->name[15] = 0;
taskdata->entry = entry;
taskdata->argc = argc;
for (i = 0; i < argc; i++) {
PX4_DEBUG("arg %d %s\n", i, argv[i]);
taskdata->argv[i] = (char *)offset;
strcpy((char *)offset, argv[i]);
offset += strlen(argv[i]) + 1;
}
// Must add NULL at end of argv
taskdata->argv[argc] = (char *)nullptr;
PX4_DEBUG("starting task %s", name);
pthread_attr_t attr;
int rv = pthread_attr_init(&attr);
if (rv != 0) {
PX4_ERR("px4_task_spawn_cmd: failed to init thread attrs");
free(taskdata);
return (rv < 0) ? rv : -rv;
}
#ifndef __PX4_DARWIN
if (stack_size < PTHREAD_STACK_MIN) {
stack_size = PTHREAD_STACK_MIN;
}
rv = pthread_attr_setstacksize(&attr, PX4_STACK_ADJUSTED(stack_size));
if (rv != 0) {
PX4_ERR("pthread_attr_setstacksize to %d returned error (%d)", stack_size, rv);
pthread_attr_destroy(&attr);
free(taskdata);
return (rv < 0) ? rv : -rv;
}
#endif
rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
if (rv != 0) {
PX4_ERR("px4_task_spawn_cmd: failed to set inherit sched");
pthread_attr_destroy(&attr);
free(taskdata);
return (rv < 0) ? rv : -rv;
}
rv = pthread_attr_setschedpolicy(&attr, scheduler);
if (rv != 0) {
PX4_ERR("px4_task_spawn_cmd: failed to set sched policy");
pthread_attr_destroy(&attr);
free(taskdata);
return (rv < 0) ? rv : -rv;
}
#ifdef __PX4_CYGWIN
/* Priorities on Windows are defined a lot differently */
priority = SCHED_PRIORITY_DEFAULT;
#endif
param.sched_priority = priority;
rv = pthread_attr_setschedparam(&attr, ¶m);
if (rv != 0) {
PX4_ERR("px4_task_spawn_cmd: failed to set sched param");
pthread_attr_destroy(&attr);
free(taskdata);
return (rv < 0) ? rv : -rv;
}
pthread_mutex_lock(&task_mutex);
px4_task_t taskid = 0;
for (i = 0; i < PX4_MAX_TASKS; ++i) {
if (!taskmap[i].isused) {
taskmap[i].name = name;
taskmap[i].isused = true;
taskid = i;
break;
}
}
if (i >= PX4_MAX_TASKS) {
pthread_attr_destroy(&attr);
pthread_mutex_unlock(&task_mutex);
free(taskdata);
return -ENOSPC;
}
rv = pthread_create(&taskmap[taskid].pid, &attr, &entry_adapter, (void *) taskdata);
if (rv != 0) {
if (rv == EPERM) {
//printf("WARNING: NOT RUNING AS ROOT, UNABLE TO RUN REALTIME THREADS\n");
rv = pthread_create(&taskmap[taskid].pid, nullptr, &entry_adapter, (void *) taskdata);
if (rv != 0) {
PX4_ERR("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno);
taskmap[taskid].isused = false;
pthread_attr_destroy(&attr);
pthread_mutex_unlock(&task_mutex);
free(taskdata);
return (rv < 0) ? rv : -rv;
}
} else {
pthread_attr_destroy(&attr);
pthread_mutex_unlock(&task_mutex);
free(taskdata);
return (rv < 0) ? rv : -rv;
}
}
pthread_attr_destroy(&attr);
pthread_mutex_unlock(&task_mutex);
return taskid;
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
int
test()
{
struct distance_sensor_s report;
ssize_t sz;
int fd = px4_open(RANGE_FINDER0_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
PX4_ERR("%s open failed (try 'tfmini start' if the driver is not running", RANGE_FINDER0_DEVICE_PATH);
return 1;
}
/* do a simple demand read */
sz = px4_read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
PX4_ERR("immediate read failed");
close(fd);
return 1;
}
print_message(report);
/* start the sensor polling at 2 Hz rate */
if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
PX4_ERR("failed to set 2Hz poll rate");
return 1;
}
/* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) {
px4_pollfd_struct_t fds{};
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
int ret = px4_poll(&fds, 1, 2000);
if (ret != 1) {
PX4_ERR("timed out");
break;
}
/* now go get it */
sz = px4_read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
PX4_ERR("read failed: got %zi vs exp. %zu", sz, sizeof(report));
break;
}
print_message(report);
}
/* reset the sensor polling to the default rate */
if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
PX4_ERR("failed to set default poll rate");
return 1;
}
PX4_INFO("PASS");
return 0;
}
int
TFMINI::collect()
{
perf_begin(_sample_perf);
/* clear buffer if last read was too long ago */
int64_t read_elapsed = hrt_elapsed_time(&_last_read);
/* the buffer for read chars is buflen minus null termination */
char readbuf[sizeof(_linebuf)];
unsigned readlen = sizeof(readbuf) - 1;
int ret = 0;
float distance_m = -1.0f;
/* Check the number of bytes available in the buffer*/
int bytes_available = 0;
::ioctl(_fd, FIONREAD, (unsigned long)&bytes_available);
if (!bytes_available) {
return -EAGAIN;
}
/* parse entire buffer */
do {
/* read from the sensor (uart buffer) */
ret = ::read(_fd, &readbuf[0], readlen);
if (ret < 0) {
PX4_ERR("read err: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
/* only throw an error if we time out */
if (read_elapsed > (_conversion_interval * 2)) {
/* flush anything in RX buffer */
tcflush(_fd, TCIFLUSH);
return ret;
} else {
return -EAGAIN;
}
}
_last_read = hrt_absolute_time();
/* parse buffer */
for (int i = 0; i < ret; i++) {
tfmini_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m);
}
/* bytes left to parse */
bytes_available -= ret;
} while (bytes_available > 0);
/* no valid measurement after parsing buffer */
if (distance_m < 0.0f) {
return -EAGAIN;
}
/* publish most recent valid measurement from buffer */
distance_sensor_s report{};
report.timestamp = hrt_absolute_time();
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
report.orientation = _rotation;
report.current_distance = distance_m;
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0f;
report.signal_quality = -1;
/* TODO: set proper ID */
report.id = 0;
/* publish it */
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
_reports->force(&report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
int
TFMINI::init()
{
int32_t hw_model = 1; // only one model so far...
switch (hw_model) {
case 1: /* TFMINI (12m, 100 Hz)*/
_min_distance = 0.3f;
_max_distance = 12.0f;
_conversion_interval = 9000;
break;
default:
PX4_ERR("invalid HW model %d.", hw_model);
return -1;
}
/* status */
int ret = 0;
do { /* create a scope to handle exit conditions using break */
/* open fd */
_fd = ::open(_port, O_RDWR | O_NOCTTY);
if (_fd < 0) {
PX4_ERR("Error opening fd");
return -1;
}
/*baudrate 115200, 8 bits, no parity, 1 stop bit */
unsigned speed = B115200;
struct termios uart_config;
int termios_state;
tcgetattr(_fd, &uart_config);
/* clear ONLCR flag (which appends a CR for every LF) */
uart_config.c_oflag &= ~ONLCR;
/* set baud rate */
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
PX4_ERR("CFG: %d ISPD", termios_state);
ret = -1;
break;
}
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
PX4_ERR("CFG: %d OSPD\n", termios_state);
ret = -1;
break;
}
if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) {
PX4_ERR("baud %d ATTR", termios_state);
ret = -1;
break;
}
uart_config.c_cflag |= (CLOCAL | CREAD); /* ignore modem controls */
uart_config.c_cflag &= ~CSIZE;
uart_config.c_cflag |= CS8; /* 8-bit characters */
uart_config.c_cflag &= ~PARENB; /* no parity bit */
uart_config.c_cflag &= ~CSTOPB; /* only need 1 stop bit */
uart_config.c_cflag &= ~CRTSCTS; /* no hardware flowcontrol */
/* setup for non-canonical mode */
uart_config.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
uart_config.c_oflag &= ~OPOST;
/* fetch bytes as they become available */
uart_config.c_cc[VMIN] = 1;
uart_config.c_cc[VTIME] = 1;
if (_fd < 0) {
PX4_ERR("FAIL: laser fd");
ret = -1;
break;
}
/* do regular cdev init */
ret = CDev::init();
if (ret != OK) { break; }
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s));
if (_reports == nullptr) {
PX4_ERR("mem err");
ret = -1;
break;
}
_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
/* get a publish handle on the range finder topic */
struct distance_sensor_s ds_report = {};
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
&_orb_class_instance, ORB_PRIO_HIGH);
if (_distance_sensor_topic == nullptr) {
PX4_ERR("failed to create distance_sensor object. Did you start uOrb?");
}
} while (0);
/* close the fd */
::close(_fd);
_fd = -1;
return ret;
}
// perform some basic functional tests on the driver;
// make sure we can collect data from the sensor in polled
// and automatic modes.
int test()
{
int fd = px4_open(PATH_SDP3X, O_RDONLY);
if (fd < 0) {
PX4_WARN("%s open failed (try 'sdp3x_airspeed start' if the driver is not running", PATH_SDP3X);
return PX4_ERROR;
}
// do a simple demand read
differential_pressure_s report;
ssize_t sz = px4_read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
PX4_WARN("immediate read failed");
return PX4_ERROR;
}
PX4_WARN("single read");
PX4_WARN("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa);
/* start the sensor polling at 2Hz */
if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
PX4_WARN("failed to set 2Hz poll rate");
return PX4_ERROR;
}
/* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) {
px4_pollfd_struct_t fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
int ret = px4_poll(&fds, 1, 2000);
if (ret != 1) {
PX4_ERR("timed out");
return PX4_ERROR;
}
/* now go get it */
sz = px4_read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
PX4_ERR("periodic read failed");
return PX4_ERROR;
}
PX4_WARN("periodic read %u", i);
PX4_WARN("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa);
PX4_WARN("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature);
}
/* reset the sensor polling to its default rate */
if (PX4_OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
PX4_WARN("failed to set default rate");
return PX4_ERROR;
}
return PX4_OK;
}
int GPS::setBaudrate(unsigned baud)
{
#if __PX4_QURT
// TODO: currently QURT does not support configuration with termios.
dspal_serial_ioctl_data_rate data_rate;
switch (baud) {
case 9600: data_rate.bit_rate = DSPAL_SIO_BITRATE_9600; break;
case 19200: data_rate.bit_rate = DSPAL_SIO_BITRATE_19200; break;
case 38400: data_rate.bit_rate = DSPAL_SIO_BITRATE_38400; break;
case 57600: data_rate.bit_rate = DSPAL_SIO_BITRATE_57600; break;
case 115200: data_rate.bit_rate = DSPAL_SIO_BITRATE_115200; break;
default:
PX4_ERR("ERR: unknown baudrate: %d", baud);
return -EINVAL;
}
int ret = ::ioctl(_serial_fd, SERIAL_IOCTL_SET_DATA_RATE, (void *)&data_rate);
if (ret != 0) {
return ret;
}
#else
/* process baud rate */
int speed;
switch (baud) {
case 9600: speed = B9600; break;
case 19200: speed = B19200; break;
case 38400: speed = B38400; break;
case 57600: speed = B57600; break;
case 115200: speed = B115200; break;
default:
PX4_ERR("ERR: unknown baudrate: %d", baud);
return -EINVAL;
}
struct termios uart_config;
int termios_state;
/* fill the struct for the new configuration */
tcgetattr(_serial_fd, &uart_config);
/* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */
//
// Input flags - Turn off input processing
//
// convert break to null byte, no CR to NL translation,
// no NL to CR translation, don't mark parity errors or breaks
// no input parity check, don't strip high bit off,
// no XON/XOFF software flow control
//
uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL |
INLCR | PARMRK | INPCK | ISTRIP | IXON);
//
// Output flags - Turn off output processing
//
// no CR to NL translation, no NL to CR-NL translation,
// no NL to CR translation, no column 0 CR suppression,
// no Ctrl-D suppression, no fill characters, no case mapping,
// no local output processing
//
// config.c_oflag &= ~(OCRNL | ONLCR | ONLRET |
// ONOCR | ONOEOT| OFILL | OLCUC | OPOST);
uart_config.c_oflag = 0;
//
// No line processing
//
// echo off, echo newline off, canonical mode off,
// extended input processing off, signal chars off
//
uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
/* no parity, one stop bit */
uart_config.c_cflag &= ~(CSTOPB | PARENB);
/* set baud rate */
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
GPS_ERR("ERR: %d (cfsetispeed)", termios_state);
return -1;
}
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
GPS_ERR("ERR: %d (cfsetospeed)", termios_state);
return -1;
}
if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
GPS_ERR("ERR: %d (tcsetattr)", termios_state);
return -1;
}
#endif
return 0;
}
int test_mixer(int argc, char *argv[])
{
/*
* PWM limit structure
*/
pwm_limit_t pwm_limit;
bool should_arm = false;
uint16_t r_page_servo_disarmed[output_max];
uint16_t r_page_servo_control_min[output_max];
uint16_t r_page_servo_control_max[output_max];
uint16_t r_page_servos[output_max];
uint16_t servo_predicted[output_max];
int16_t reverse_pwm_mask = 0;
//PX4_INFO("testing mixer");
#if !defined(CONFIG_ARCH_BOARD_SITL)
const char *filename = "/etc/mixers/IO_pass.mix";
#else
const char *filename = "ROMFS/px4fmu_test/mixers/IO_pass.mix";
#endif
//PX4_INFO("loading: %s", filename);
char buf[2048];
load_mixer_file(filename, &buf[0], sizeof(buf));
unsigned loaded = strlen(buf);
//fprintf(stderr, "loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);
/* load the mixer in chunks, like
* in the case of a remote load,
* e.g. on PX4IO.
*/
const unsigned chunk_size = 64;
MixerGroup mixer_group(mixer_callback, 0);
/* load at once test */
unsigned xx = loaded;
mixer_group.load_from_buf(&buf[0], xx);
//ASSERT_EQ(mixer_group.count(), 8);
unsigned empty_load = 2;
char empty_buf[2];
empty_buf[0] = ' ';
empty_buf[1] = '\0';
mixer_group.reset();
mixer_group.load_from_buf(&empty_buf[0], empty_load);
//PX4_INFO("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load);
//ASSERT_NE(empty_load, 0);
if (empty_load != 0) {
return 1;
}
/* FIRST mark the mixer as invalid */
/* THEN actually delete it */
mixer_group.reset();
char mixer_text[256]; /* large enough for one mixer */
unsigned mixer_text_length = 0;
unsigned transmitted = 0;
//PX4_INFO("transmitted: %d, loaded: %d", transmitted, loaded);
while (transmitted < loaded) {
unsigned text_length = (loaded - transmitted > chunk_size) ? chunk_size : loaded - transmitted;
/* check for overflow - this would be really fatal */
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
return 1;
}
/* append mixer text and nul-terminate */
memcpy(&mixer_text[mixer_text_length], &buf[transmitted], text_length);
mixer_text_length += text_length;
mixer_text[mixer_text_length] = '\0';
//fprintf(stderr, "buflen %u, text:\n\"%s\"", mixer_text_length, &mixer_text[0]);
/* process the text buffer, adding new mixers as their descriptions can be parsed */
unsigned resid = mixer_text_length;
mixer_group.load_from_buf(&mixer_text[0], resid);
/* if anything was parsed */
if (resid != mixer_text_length) {
//fprintf(stderr, "used %u", mixer_text_length - resid);
/* copy any leftover text to the base of the buffer for re-use */
if (resid > 0) {
memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
}
mixer_text_length = resid;
}
transmitted += text_length;
}
//PX4_INFO("chunked load: loaded %u mixers", mixer_group.count());
if (mixer_group.count() != 8) {
return 1;
}
/* execute the mixer */
float outputs[output_max];
unsigned mixed;
const int jmax = 5;
pwm_limit_init(&pwm_limit);
/* run through arming phase */
for (unsigned i = 0; i < output_max; i++) {
actuator_controls[i] = 0.1f;
r_page_servo_disarmed[i] = PWM_MOTOR_OFF;
r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
}
//PX4_INFO("PRE-ARM TEST: DISABLING SAFETY");
/* mix */
should_prearm = true;
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {
//fprintf(stderr, "pre-arm:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
if (i != actuator_controls_s::INDEX_THROTTLE) {
if (r_page_servos[i] < r_page_servo_control_min[i]) {
warnx("active servo < min");
return 1;
}
} else {
if (r_page_servos[i] != r_page_servo_disarmed[i]) {
warnx("throttle output != 0 (this check assumed the IO pass mixer!)");
return 1;
}
}
}
should_arm = true;
should_prearm = false;
/* simulate another orb_copy() from actuator controls */
for (unsigned i = 0; i < output_max; i++) {
actuator_controls[i] = 0.1f;
}
//PX4_INFO("ARMING TEST: STARTING RAMP");
unsigned sleep_quantum_us = 10000;
hrt_abstime starttime = hrt_absolute_time();
unsigned sleepcount = 0;
while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US + 2 * sleep_quantum_us) {
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {
//fprintf(stderr, "ramp:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
/* check mixed outputs to be zero during init phase */
if (hrt_elapsed_time(&starttime) < INIT_TIME_US &&
r_page_servos[i] != r_page_servo_disarmed[i]) {
PX4_ERR("disarmed servo value mismatch: %d vs %d", r_page_servos[i], r_page_servo_disarmed[i]);
return 1;
}
if (hrt_elapsed_time(&starttime) >= INIT_TIME_US &&
r_page_servos[i] + 1 <= r_page_servo_disarmed[i]) {
PX4_ERR("ramp servo value mismatch");
return 1;
}
}
usleep(sleep_quantum_us);
sleepcount++;
if (sleepcount % 10 == 0) {
fflush(stdout);
}
}
//PX4_INFO("ARMING TEST: NORMAL OPERATION");
for (int j = -jmax; j <= jmax; j++) {
for (unsigned i = 0; i < output_max; i++) {
actuator_controls[i] = j / 10.0f + 0.1f * i;
r_page_servo_disarmed[i] = PWM_LOWEST_MIN;
r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
}
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//fprintf(stderr, "mixed %d outputs (max %d)", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {
servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
if (abs(servo_predicted[i] - r_page_servos[i]) > MIXER_DIFFERENCE_THRESHOLD) {
fprintf(stderr, "\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i],
(int)r_page_servos[i]);
PX4_ERR("mixer violated predicted value");
return 1;
}
}
}
//PX4_INFO("ARMING TEST: DISARMING");
starttime = hrt_absolute_time();
sleepcount = 0;
should_arm = false;
while (hrt_elapsed_time(&starttime) < 600000) {
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {
//fprintf(stderr, "disarmed:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
/* check mixed outputs to be zero during init phase */
if (r_page_servos[i] != r_page_servo_disarmed[i]) {
PX4_ERR("disarmed servo value mismatch");
return 1;
}
}
usleep(sleep_quantum_us);
sleepcount++;
if (sleepcount % 10 == 0) {
//printf(".");
//fflush(stdout);
}
}
//printf("\n");
//PX4_INFO("ARMING TEST: REARMING: STARTING RAMP");
starttime = hrt_absolute_time();
sleepcount = 0;
should_arm = true;
while (hrt_elapsed_time(&starttime) < 600000 + RAMP_TIME_US) {
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {
/* predict value */
servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
/* check ramp */
//fprintf(stderr, "ramp:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
if (hrt_elapsed_time(&starttime) < RAMP_TIME_US &&
(r_page_servos[i] + 1 <= r_page_servo_disarmed[i] ||
r_page_servos[i] > servo_predicted[i])) {
PX4_ERR("ramp servo value mismatch");
return 1;
}
/* check post ramp phase */
if (hrt_elapsed_time(&starttime) > RAMP_TIME_US &&
abs(servo_predicted[i] - r_page_servos[i]) > 2) {
printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]);
PX4_ERR("mixer violated predicted value");
return 1;
}
}
usleep(sleep_quantum_us);
sleepcount++;
if (sleepcount % 10 == 0) {
// printf(".");
// fflush(stdout);
}
}
//printf("\n");
/* load multirotor at once test */
mixer_group.reset();
#if !defined(CONFIG_ARCH_BOARD_SITL)
filename = "/etc/mixers/quad_test.mix";
#else
filename = "ROMFS/px4fmu_test/mixers/quad_test.mix";
#endif
load_mixer_file(filename, &buf[0], sizeof(buf));
loaded = strlen(buf);
//fprintf(stderr, "loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);
unsigned mc_loaded = loaded;
mixer_group.load_from_buf(&buf[0], mc_loaded);
//PX4_INFO("complete buffer load: loaded %u mixers", mixer_group.count());
if (mixer_group.count() != 5) {
PX4_ERR("FAIL: Quad test mixer load failed");
return 1;
}
//PX4_INFO("SUCCESS: No errors in mixer test");
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);
}
#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);
}
px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const argv[])
{
int rv;
int argc = 0;
int i;
unsigned int len = 0;
unsigned long offset;
unsigned long structsize;
char * p = (char *)argv;
pthread_t task;
pthread_attr_t attr;
struct sched_param param;
// Calculate argc
while (p != (char *)0) {
p = argv[argc];
if (p == (char *)0)
break;
++argc;
len += strlen(p)+1;
}
structsize = sizeof(pthdata_t)+(argc+1)*sizeof(char *);
pthdata_t *taskdata;
// not safe to pass stack data to the thread creation
taskdata = (pthdata_t *)malloc(structsize+len);
offset = ((unsigned long)taskdata)+structsize;
taskdata->entry = entry;
taskdata->argc = argc;
for (i=0; i<argc; i++) {
printf("arg %d %s\n", i, argv[i]);
taskdata->argv[i] = (char *)offset;
strcpy((char *)offset, argv[i]);
offset+=strlen(argv[i])+1;
}
// Must add NULL at end of argv
taskdata->argv[argc] = (char *)0;
rv = pthread_attr_init(&attr);
if (rv != 0) {
PX4_WARN("px4_task_spawn_cmd: failed to init thread attrs");
return (rv < 0) ? rv : -rv;
}
rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
if (rv != 0) {
PX4_WARN("px4_task_spawn_cmd: failed to set inherit sched");
return (rv < 0) ? rv : -rv;
}
rv = pthread_attr_setschedpolicy(&attr, scheduler);
if (rv != 0) {
PX4_WARN("px4_task_spawn_cmd: failed to set sched policy");
return (rv < 0) ? rv : -rv;
}
param.sched_priority = priority;
rv = pthread_attr_setschedparam(&attr, ¶m);
if (rv != 0) {
PX4_WARN("px4_task_spawn_cmd: failed to set sched param");
return (rv < 0) ? rv : -rv;
}
rv = pthread_create (&task, &attr, &entry_adapter, (void *) taskdata);
if (rv != 0) {
if (rv == EPERM) {
//printf("WARNING: NOT RUNING AS ROOT, UNABLE TO RUN REALTIME THREADS\n");
rv = pthread_create (&task, NULL, &entry_adapter, (void *) taskdata);
if (rv != 0) {
PX4_ERR("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno);
return (rv < 0) ? rv : -rv;
}
}
else {
return (rv < 0) ? rv : -rv;
}
}
for (i=0; i<PX4_MAX_TASKS; ++i) {
if (taskmap[i].isused == false) {
taskmap[i].pid = task;
taskmap[i].name = name;
taskmap[i].isused = true;
break;
}
}
if (i>=PX4_MAX_TASKS) {
return -ENOSPC;
}
return i;
}
int
l3gd20_main(int argc, char *argv[])
{
int myoptind = 1;
int ch;
const char *myoptarg = nullptr;
bool external_bus = false;
enum Rotation rotation = ROTATION_NONE;
while ((ch = px4_getopt(argc, argv, "XR:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'X':
external_bus = true;
break;
case 'R':
rotation = (enum Rotation)atoi(myoptarg);
break;
default:
l3gd20::usage();
return 0;
}
}
if (myoptind >= argc) {
l3gd20::usage();
return -1;
}
const char *verb = argv[myoptind];
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
l3gd20::start(external_bus, rotation);
}
/*
* Test the driver/device.
*/
if (!strcmp(verb, "test")) {
l3gd20::test();
}
/*
* Reset the driver.
*/
if (!strcmp(verb, "reset")) {
l3gd20::reset();
}
/*
* Print driver information.
*/
if (!strcmp(verb, "info")) {
l3gd20::info();
}
/*
* Print register information.
*/
if (!strcmp(verb, "regdump")) {
l3gd20::regdump();
}
/*
* trigger an error
*/
if (!strcmp(verb, "testerror")) {
l3gd20::test_error();
}
PX4_ERR("unrecognized command, try 'start', 'test', 'reset', 'info', 'testerror' or 'regdump'");
return -1;
}
/// @brief Responds to a List command
MavlinkFTP::ErrorCode
MavlinkFTP::_workList(PayloadHeader *payload, bool list_hidden)
{
strncpy(_work_buffer1, _root_dir, _work_buffer1_len);
strncpy(_work_buffer1 + _root_dir_len, _data_as_cstring(payload), _work_buffer1_len - _root_dir_len);
// ensure termination
_work_buffer1[_work_buffer1_len - 1] = '\0';
ErrorCode errorCode = kErrNone;
unsigned offset = 0;
DIR *dp = opendir(_work_buffer1);
if (dp == nullptr) {
#ifdef MAVLINK_FTP_UNIT_TEST
warnx("File open failed");
#else
_mavlink->send_statustext_critical("FTP: can't open path (file system corrupted?)");
_mavlink->send_statustext_critical(_work_buffer1);
#endif
// this is not an FTP error, abort directory by simulating eof
return kErrEOF;
}
#ifdef MAVLINK_FTP_DEBUG
warnx("FTP: list %s offset %d", _work_buffer1, payload->offset);
#endif
struct dirent *result = nullptr;
// move to the requested offset
int requested_offset = payload->offset;
while (requested_offset-- > 0 && readdir(dp));
for (;;) {
errno = 0;
result = readdir(dp);
// read the directory entry
if (result == nullptr) {
if (errno) {
#ifdef MAVLINK_FTP_UNIT_TEST
warnx("readdir failed");
#else
_mavlink->send_statustext_critical("FTP: list readdir failure");
_mavlink->send_statustext_critical(_work_buffer1);
#endif
payload->data[offset++] = kDirentSkip;
*((char *)&payload->data[offset]) = '\0';
offset++;
payload->size = offset;
closedir(dp);
return errorCode;
}
// no more entries?
if (payload->offset != 0 && offset == 0) {
// User is requesting subsequent dir entries but there were none. This means the user asked
// to seek past EOF.
errorCode = kErrEOF;
}
// Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that
break;
}
uint32_t fileSize = 0;
char direntType;
// Determine the directory entry type
switch (result->d_type) {
#ifdef __PX4_NUTTX
case DTYPE_FILE: {
#else
case DT_REG: {
#endif
// For files we get the file size as well
direntType = kDirentFile;
int ret = snprintf(_work_buffer2, _work_buffer2_len, "%s/%s", _work_buffer1, result->d_name);
bool buf_is_ok = ((ret > 0) && (ret < _work_buffer2_len));
if (buf_is_ok) {
struct stat st;
if (stat(_work_buffer2, &st) == 0) {
fileSize = st.st_size;
}
}
break;
}
#ifdef __PX4_NUTTX
case DTYPE_DIRECTORY:
#else
case DT_DIR:
#endif
if ((!list_hidden && (strncmp(result->d_name, ".", 1) == 0)) ||
strcmp(result->d_name, ".") == 0 || strcmp(result->d_name, "..") == 0) {
// Don't bother sending these back
direntType = kDirentSkip;
} else {
direntType = kDirentDir;
}
break;
default:
// We only send back file and diretory entries, skip everything else
direntType = kDirentSkip;
}
if (direntType == kDirentSkip) {
// Skip send only dirent identifier
_work_buffer2[0] = '\0';
} else if (direntType == kDirentFile) {
// Files send filename and file length
int ret = snprintf(_work_buffer2, _work_buffer2_len, "%s\t%d", result->d_name, fileSize);
bool buf_is_ok = ((ret > 0) && (ret < _work_buffer2_len));
if (!buf_is_ok) {
_work_buffer2[_work_buffer2_len - 1] = '\0';
}
} else {
// Everything else just sends name
strncpy(_work_buffer2, result->d_name, _work_buffer2_len);
_work_buffer2[_work_buffer2_len - 1] = '\0';
}
size_t nameLen = strlen(_work_buffer2);
// Do we have room for the name, the one char directory identifier and the null terminator?
if ((offset + nameLen + 2) > kMaxDataLength) {
break;
}
// Move the data into the buffer
payload->data[offset++] = direntType;
strcpy((char *)&payload->data[offset], _work_buffer2);
#ifdef MAVLINK_FTP_DEBUG
printf("FTP: list %s %s\n", _work_buffer1, (char *)&payload->data[offset - 1]);
#endif
offset += nameLen + 1;
}
closedir(dp);
payload->size = offset;
return errorCode;
}
/// @brief Responds to an Open command
MavlinkFTP::ErrorCode
MavlinkFTP::_workOpen(PayloadHeader *payload, int oflag)
{
if (_session_info.fd >= 0) {
PX4_ERR("FTP: Open failed - out of sessions\n");
return kErrNoSessionsAvailable;
}
strncpy(_work_buffer1, _root_dir, _work_buffer1_len);
strncpy(_work_buffer1 + _root_dir_len, _data_as_cstring(payload), _work_buffer1_len - _root_dir_len);
#ifdef MAVLINK_FTP_DEBUG
warnx("FTP: open '%s'", _work_buffer1);
#endif
uint32_t fileSize = 0;
struct stat st;
if (stat(_work_buffer1, &st) != 0) {
// fail only if requested open for read
if (oflag & O_RDONLY) {
return kErrFailErrno;
} else {
st.st_size = 0;
}
}
fileSize = st.st_size;
// Set mode to 666 incase oflag has O_CREAT
int fd = ::open(_work_buffer1, oflag, PX4_O_MODE_666);
if (fd < 0) {
return kErrFailErrno;
}
_session_info.fd = fd;
_session_info.file_size = fileSize;
_session_info.stream_download = false;
payload->session = 0;
payload->size = sizeof(uint32_t);
std::memcpy(payload->data, &fileSize, payload->size);
return kErrNone;
}
/// @brief Responds to a Read command
MavlinkFTP::ErrorCode
MavlinkFTP::_workRead(PayloadHeader *payload)
{
if (payload->session != 0 || _session_info.fd < 0) {
return kErrInvalidSession;
}
#ifdef MAVLINK_FTP_DEBUG
warnx("FTP: read offset:%d", payload->offset);
#endif
// We have to test seek past EOF ourselves, lseek will allow seek past EOF
if (payload->offset >= _session_info.file_size) {
PX4_ERR("request past EOF");
return kErrEOF;
}
if (lseek(_session_info.fd, payload->offset, SEEK_SET) < 0) {
PX4_ERR("seek fail");
return kErrFailErrno;
}
int bytes_read = ::read(_session_info.fd, &payload->data[0], kMaxDataLength);
if (bytes_read < 0) {
// Negative return indicates error other than eof
PX4_ERR("read fail %d", bytes_read);
return kErrFailErrno;
}
payload->size = bytes_read;
return kErrNone;
}
/// @brief Responds to a Stream command
MavlinkFTP::ErrorCode
MavlinkFTP::_workBurst(PayloadHeader *payload, uint8_t target_system_id)
{
if (payload->session != 0 && _session_info.fd < 0) {
return kErrInvalidSession;
}
#ifdef MAVLINK_FTP_DEBUG
warnx("FTP: burst offset:%d", payload->offset);
#endif
// Setup for streaming sends
_session_info.stream_download = true;
_session_info.stream_offset = payload->offset;
_session_info.stream_chunk_transmitted = 0;
_session_info.stream_seq_number = payload->seq_number + 1;
_session_info.stream_target_system_id = target_system_id;
return kErrNone;
}
/// @brief Responds to a Write command
MavlinkFTP::ErrorCode
MavlinkFTP::_workWrite(PayloadHeader *payload)
{
if (payload->session != 0 && _session_info.fd < 0) {
return kErrInvalidSession;
}
if (lseek(_session_info.fd, payload->offset, SEEK_SET) < 0) {
// Unable to see to the specified location
PX4_ERR("seek fail");
return kErrFailErrno;
}
int bytes_written = ::write(_session_info.fd, &payload->data[0], payload->size);
if (bytes_written < 0) {
// Negative return indicates error other than eof
PX4_ERR("write fail %d", bytes_written);
return kErrFailErrno;
}
payload->size = sizeof(uint32_t);
std::memcpy(payload->data, &bytes_written, payload->size);
return kErrNone;
}
/// @brief Responds to a RemoveFile command
MavlinkFTP::ErrorCode
MavlinkFTP::_workRemoveFile(PayloadHeader *payload)
{
strncpy(_work_buffer1, _root_dir, _work_buffer1_len);
strncpy(_work_buffer1 + _root_dir_len, _data_as_cstring(payload), _work_buffer1_len - _root_dir_len);
// ensure termination
_work_buffer1[_work_buffer1_len - 1] = '\0';
if (unlink(_work_buffer1) == 0) {
payload->size = 0;
return kErrNone;
} else {
return kErrFailErrno;
}
}
bool MixerTest::loadAllTest()
{
PX4_INFO("Testing all mixers in %s", MIXER_ONBOARD_PATH);
DIR *dp = opendir(MIXER_ONBOARD_PATH);
if (dp == nullptr) {
PX4_ERR("File open failed");
return false;
}
struct dirent *result = nullptr;
for (;;) {
errno = 0;
result = readdir(dp);
// read the directory entry
if (result == nullptr) {
if (errno) {
PX4_ERR("readdir failed");
closedir(dp);
return false;
}
// We are just at the last directory entry
break;
}
// Determine the directory entry type
switch (result->d_type) {
#ifdef __PX4_NUTTX
case DTYPE_FILE:
#else
case DT_REG:
#endif
if (strncmp(result->d_name, ".", 1) != 0) {
char buf[PATH_MAX];
if (snprintf(buf, PATH_MAX, "%s/%s", MIXER_ONBOARD_PATH, result->d_name) >= PATH_MAX) {
PX4_ERR("mixer path too long %s", result->d_name);
closedir(dp);
return false;
}
bool ret = load_mixer(buf, 0);
if (!ret) {
PX4_ERR("Error testing mixer %s", buf);
closedir(dp);
return false;
}
}
break;
default:
break;
}
}
closedir(dp);
return true;
}
CameraTrigger::CameraTrigger() :
_engagecall {},
_disengagecall {},
_engage_turn_on_off_call {},
_disengage_turn_on_off_call {},
_keepalivecall_up {},
_keepalivecall_down {},
_activation_time(0.5f /* ms */),
_interval(100.0f /* ms */),
_distance(25.0f /* m */),
_trigger_seq(0),
_trigger_enabled(false),
_trigger_paused(false),
_one_shot(false),
_test_shot(false),
_turning_on(false),
_last_shoot_position(0.0f, 0.0f),
_valid_position(false),
_command_sub(-1),
_lpos_sub(-1),
_trigger_pub(nullptr),
_cmd_ack_pub(nullptr),
_trigger_mode(TRIGGER_MODE_NONE),
_camera_interface_mode(CAMERA_INTERFACE_MODE_GPIO),
_camera_interface(nullptr)
{
// Initiate camera interface based on camera_interface_mode
if (_camera_interface != nullptr) {
delete (_camera_interface);
// set to zero to ensure parser is not used while not instantiated
_camera_interface = nullptr;
}
memset(&_work, 0, sizeof(_work));
// Parameters
_p_interval = param_find("TRIG_INTERVAL");
_p_distance = param_find("TRIG_DISTANCE");
_p_activation_time = param_find("TRIG_ACT_TIME");
_p_mode = param_find("TRIG_MODE");
_p_interface = param_find("TRIG_INTERFACE");
param_get(_p_activation_time, &_activation_time);
param_get(_p_interval, &_interval);
param_get(_p_distance, &_distance);
param_get(_p_mode, &_trigger_mode);
param_get(_p_interface, &_camera_interface_mode);
switch (_camera_interface_mode) {
#ifdef __PX4_NUTTX
case CAMERA_INTERFACE_MODE_GPIO:
_camera_interface = new CameraInterfaceGPIO();
break;
case CAMERA_INTERFACE_MODE_GENERIC_PWM:
_camera_interface = new CameraInterfacePWM();
break;
case CAMERA_INTERFACE_MODE_SEAGULL_MAP2_PWM:
_camera_interface = new CameraInterfaceSeagull();
break;
#endif
case CAMERA_INTERFACE_MODE_MAVLINK:
// start an interface that does nothing. Instead mavlink will listen to the camera_trigger uORB message
_camera_interface = new CameraInterface();
break;
default:
PX4_ERR("unknown camera interface mode: %i", (int)_camera_interface_mode);
break;
}
// Enforce a lower bound on the activation interval in PWM modes to not miss
// engage calls in-between 50Hz PWM pulses. (see PX4 PR #6973)
if ((_activation_time < 40.0f) &&
(_camera_interface_mode == CAMERA_INTERFACE_MODE_GENERIC_PWM ||
_camera_interface_mode == CAMERA_INTERFACE_MODE_SEAGULL_MAP2_PWM)) {
_activation_time = 40.0f;
PX4_WARN("Trigger interval too low for PWM interface, setting to 40 ms");
param_set(_p_activation_time, &(_activation_time));
}
// Advertise critical publishers here, because we cannot advertise in interrupt context
struct camera_trigger_s trigger = {};
_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger);
}
bool MixerTest::load_mixer(const char *filename, const char *buf, unsigned loaded, unsigned expected_count,
const unsigned chunk_size,
bool verbose)
{
/* load the mixer in chunks, like
* in the case of a remote load,
* e.g. on PX4IO.
*/
/* load at once test */
unsigned xx = loaded;
mixer_group.reset();
mixer_group.load_from_buf(&buf[0], xx);
if (expected_count > 0) {
ut_compare("check number of mixers loaded", mixer_group.count(), expected_count);
}
unsigned empty_load = 2;
char empty_buf[2];
empty_buf[0] = ' ';
empty_buf[1] = '\0';
mixer_group.reset();
mixer_group.load_from_buf(&empty_buf[0], empty_load);
if (verbose) {
PX4_INFO("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load);
}
ut_compare("empty buffer load", empty_load, 0);
/* reset, load in chunks */
mixer_group.reset();
char mixer_text[PX4IO_MAX_MIXER_LENGTH]; /* large enough for one mixer */
unsigned mixer_text_length = 0;
unsigned transmitted = 0;
unsigned resid = 0;
while (transmitted < loaded) {
unsigned text_length = (loaded - transmitted > chunk_size) ? chunk_size : loaded - transmitted;
/* check for overflow - this would be really fatal */
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
PX4_ERR("Mixer text length overflow for file: %s. Is PX4IO_MAX_MIXER_LENGTH too small? (curr len: %d)", filename,
PX4IO_MAX_MIXER_LENGTH);
return false;
}
/* append mixer text and nul-terminate */
memcpy(&mixer_text[mixer_text_length], &buf[transmitted], text_length);
mixer_text_length += text_length;
mixer_text[mixer_text_length] = '\0';
//fprintf(stderr, "buflen %u, text:\n\"%s\"\n", mixer_text_length, &mixer_text[0]);
/* process the text buffer, adding new mixers as their descriptions can be parsed */
resid = mixer_text_length;
mixer_group.load_from_buf(&mixer_text[0], resid);
/* if anything was parsed */
if (resid != mixer_text_length) {
//PX4_INFO("loaded %d mixers, used %u\n", mixer_group.count(), mixer_text_length - resid);
/* copy any leftover text to the base of the buffer for re-use */
if (resid > 0) {
memmove(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
/* enforce null termination */
mixer_text[resid] = '\0';
}
mixer_text_length = resid;
}
transmitted += text_length;
if (verbose) {
PX4_INFO("transmitted: %d, loaded: %d", transmitted, loaded);
}
}
if (verbose) {
PX4_INFO("chunked load: loaded %u mixers", mixer_group.count());
}
if (expected_count > 0 && mixer_group.count() != expected_count) {
PX4_ERR("Load of mixer failed, last chunk: %s, transmitted: %u, text length: %u, resid: %u", mixer_text, transmitted,
mixer_text_length, resid);
ut_compare("check number of mixers loaded (chunk)", mixer_group.count(), expected_count);
}
return true;
}
void uORB::DeviceMaster::addNewDeviceNodes(DeviceNodeStatisticsData **first_node, int &num_topics,
size_t &max_topic_name_length, char **topic_filter, int num_filters)
{
DeviceNodeStatisticsData *cur_node = nullptr;
num_topics = 0;
DeviceNodeStatisticsData *last_node = *first_node;
if (last_node) {
while (last_node->next) {
last_node = last_node->next;
}
}
ITERATE_NODE_MAP() {
INIT_NODE_MAP_VARS(node, node_name)
++num_topics;
//check if already added
cur_node = *first_node;
while (cur_node && cur_node->node != node) {
cur_node = cur_node->next;
}
if (cur_node) {
continue;
}
if (num_filters > 0 && topic_filter) {
bool matched = false;
for (int i = 0; i < num_filters; ++i) {
if (strstr(node->get_meta()->o_name, topic_filter[i])) {
matched = true;
}
}
if (!matched) {
continue;
}
}
if (last_node) {
last_node->next = new DeviceNodeStatisticsData();
last_node = last_node->next;
} else {
*first_node = last_node = new DeviceNodeStatisticsData();
}
if (!last_node) {
PX4_ERR("mem alloc failed");
break;
}
last_node->node = node;
int node_name_len = strlen(node_name);
last_node->instance = (uint8_t)(node_name[node_name_len - 1] - '0');
size_t name_length = strlen(last_node->node->get_meta()->o_name);
if (name_length > max_topic_name_length) {
max_topic_name_length = name_length;
}
last_node->last_lost_msg_count = last_node->node->lost_message_count();
last_node->last_pub_msg_count = last_node->node->published_message_count();
}
}
bool MixerTest::mixerTest()
{
/*
* PWM limit structure
*/
pwm_limit_t pwm_limit;
bool should_arm = false;
uint16_t r_page_servo_disarmed[output_max];
uint16_t r_page_servo_control_min[output_max];
uint16_t r_page_servo_control_max[output_max];
uint16_t r_page_servos[output_max];
uint16_t servo_predicted[output_max];
int16_t reverse_pwm_mask = 0;
bool load_ok = load_mixer(MIXER_PATH(IO_pass.mix), 8);
if (!load_ok) {
return load_ok;
}
/* execute the mixer */
float outputs[output_max];
unsigned mixed;
const int jmax = 5;
pwm_limit_init(&pwm_limit);
/* run through arming phase */
for (unsigned i = 0; i < output_max; i++) {
actuator_controls[i] = 0.1f;
r_page_servo_disarmed[i] = PWM_MOTOR_OFF;
r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
}
//PX4_INFO("PRE-ARM TEST: DISABLING SAFETY");
/* mix */
should_prearm = true;
mixed = mixer_group.mix(&outputs[0], output_max);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {
//fprintf(stderr, "pre-arm:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
if (i != actuator_controls_s::INDEX_THROTTLE) {
if (r_page_servos[i] < r_page_servo_control_min[i]) {
warnx("active servo < min");
return false;
}
} else {
if (r_page_servos[i] != r_page_servo_disarmed[i]) {
warnx("throttle output != 0 (this check assumed the IO pass mixer!)");
return false;
}
}
}
should_arm = true;
should_prearm = false;
/* simulate another orb_copy() from actuator controls */
for (unsigned i = 0; i < output_max; i++) {
actuator_controls[i] = 0.1f;
}
//PX4_INFO("ARMING TEST: STARTING RAMP");
unsigned sleep_quantum_us = 10000;
hrt_abstime starttime = hrt_absolute_time();
unsigned sleepcount = 0;
while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US + 2 * sleep_quantum_us) {
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {
//fprintf(stderr, "ramp:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
/* check mixed outputs to be zero during init phase */
if (hrt_elapsed_time(&starttime) < INIT_TIME_US &&
r_page_servos[i] != r_page_servo_disarmed[i]) {
PX4_ERR("disarmed servo value mismatch: %d vs %d", r_page_servos[i], r_page_servo_disarmed[i]);
return false;
}
if (hrt_elapsed_time(&starttime) >= INIT_TIME_US &&
r_page_servos[i] + 1 <= r_page_servo_disarmed[i]) {
PX4_ERR("ramp servo value mismatch");
return false;
}
}
usleep(sleep_quantum_us);
sleepcount++;
if (sleepcount % 10 == 0) {
fflush(stdout);
}
}
//PX4_INFO("ARMING TEST: NORMAL OPERATION");
for (int j = -jmax; j <= jmax; j++) {
for (unsigned i = 0; i < output_max; i++) {
actuator_controls[i] = j / 10.0f + 0.1f * i;
r_page_servo_disarmed[i] = PWM_LOWEST_MIN;
r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
}
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//fprintf(stderr, "mixed %d outputs (max %d)", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {
servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
if (abs(servo_predicted[i] - r_page_servos[i]) > MIXER_DIFFERENCE_THRESHOLD) {
fprintf(stderr, "\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i],
(int)r_page_servos[i]);
PX4_ERR("mixer violated predicted value");
return false;
}
}
}
//PX4_INFO("ARMING TEST: DISARMING");
starttime = hrt_absolute_time();
sleepcount = 0;
should_arm = false;
while (hrt_elapsed_time(&starttime) < 600000) {
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {
//fprintf(stderr, "disarmed:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
/* check mixed outputs to be zero during init phase */
if (r_page_servos[i] != r_page_servo_disarmed[i]) {
PX4_ERR("disarmed servo value mismatch");
return false;
}
}
usleep(sleep_quantum_us);
sleepcount++;
if (sleepcount % 10 == 0) {
//printf(".");
//fflush(stdout);
}
}
//printf("\n");
//PX4_INFO("ARMING TEST: REARMING: STARTING RAMP");
starttime = hrt_absolute_time();
sleepcount = 0;
should_arm = true;
while (hrt_elapsed_time(&starttime) < 600000 + RAMP_TIME_US) {
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (unsigned i = 0; i < mixed; i++) {
/* predict value */
servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
/* check ramp */
//fprintf(stderr, "ramp:\t %d: out: %8.4f, servo: %d \n", i, (double)outputs[i], (int)r_page_servos[i]);
if (hrt_elapsed_time(&starttime) < RAMP_TIME_US &&
(r_page_servos[i] + 1 <= r_page_servo_disarmed[i] ||
r_page_servos[i] > servo_predicted[i])) {
PX4_ERR("ramp servo value mismatch");
return false;
}
/* check post ramp phase */
if (hrt_elapsed_time(&starttime) > RAMP_TIME_US &&
abs(servo_predicted[i] - r_page_servos[i]) > 2) {
printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]);
PX4_ERR("mixer violated predicted value");
return false;
}
}
usleep(sleep_quantum_us);
sleepcount++;
if (sleepcount % 10 == 0) {
// printf(".");
// fflush(stdout);
}
}
return true;
}
/**
* @brief Starts the driver.
*/
void start(enum LL40LS_BUS busid, uint8_t rotation)
{
int fd, ret;
if (instance) {
PX4_INFO("driver already started");
}
if (busid == LL40LS_BUS_PWM) {
instance = new LidarLitePWM(LL40LS_DEVICE_PATH_PWM, rotation);
if (!instance) {
PX4_ERR("Failed to instantiate LidarLitePWM");
return;
}
if (instance->init() != PX4_OK) {
PX4_ERR("failed to initialize LidarLitePWM");
stop();
return;
}
} else {
for (uint8_t i = 0; i < (sizeof(bus_options) / sizeof(bus_options[0])); i++) {
if (busid != LL40LS_BUS_I2C_ALL && busid != bus_options[i].busid) {
continue;
}
instance = new LidarLiteI2C(bus_options[i].busnum, bus_options[i].devname, rotation);
if (!instance) {
PX4_ERR("Failed to instantiate LidarLiteI2C");
return;
}
if (instance->init() == PX4_OK) {
break;
}
PX4_ERR("failed to initialize LidarLiteI2C on busnum=%u", bus_options[i].busnum);
stop();
return;
}
}
if (!instance) {
PX4_WARN("No LidarLite found");
return;
}
fd = px4_open(instance->get_dev_name(), O_RDONLY);
if (fd == -1) {
PX4_ERR("Error opening fd");
stop();
return;
}
ret = px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
px4_close(fd);
if (ret < 0) {
PX4_ERR("pollrate fail");
stop();
return;
}
}
int do_accel_calibration(orb_advert_t *mavlink_log_pub)
{
#ifdef __PX4_NUTTX
int fd;
#endif
calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
struct accel_calibration_s accel_scale;
accel_scale.x_offset = 0.0f;
accel_scale.x_scale = 1.0f;
accel_scale.y_offset = 0.0f;
accel_scale.y_scale = 1.0f;
accel_scale.z_offset = 0.0f;
accel_scale.z_scale = 1.0f;
int res = PX4_OK;
char str[30];
/* reset all sensors */
for (unsigned s = 0; s < max_accel_sens; s++) {
#ifdef __PX4_NUTTX
sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
/* reset all offsets to zero and all scales to one */
fd = px4_open(str, 0);
if (fd < 0) {
continue;
}
device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
px4_close(fd);
if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, s);
}
#else
(void)sprintf(str, "CAL_ACC%u_XOFF", s);
res = param_set_no_notification(param_find(str), &accel_scale.x_offset);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_YOFF", s);
res = param_set_no_notification(param_find(str), &accel_scale.y_offset);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_ZOFF", s);
res = param_set_no_notification(param_find(str), &accel_scale.z_offset);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_XSCALE", s);
res = param_set_no_notification(param_find(str), &accel_scale.x_scale);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_YSCALE", s);
res = param_set_no_notification(param_find(str), &accel_scale.y_scale);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
(void)sprintf(str, "CAL_ACC%u_ZSCALE", s);
res = param_set_no_notification(param_find(str), &accel_scale.z_scale);
if (res != PX4_OK) {
PX4_ERR("unable to reset %s", str);
}
param_notify_changes();
#endif
}
float accel_offs[max_accel_sens][3];
float accel_T[max_accel_sens][3][3];
unsigned active_sensors = 0;
/* measure and calculate offsets & scales */
if (res == PX4_OK) {
calibrate_return cal_return = do_accel_calibration_measurements(mavlink_log_pub, accel_offs, accel_T, &active_sensors);
if (cal_return == calibrate_return_cancelled) {
// Cancel message already displayed, nothing left to do
return PX4_ERROR;
} else if (cal_return == calibrate_return_ok) {
res = PX4_OK;
} else {
res = PX4_ERROR;
}
}
if (res != PX4_OK) {
if (active_sensors == 0) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
}
return PX4_ERROR;
}
/* measurements completed successfully, rotate calibration values */
param_t board_rotation_h = param_find("SENS_BOARD_ROT");
int32_t board_rotation_int;
param_get(board_rotation_h, &(board_rotation_int));
enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
matrix::Dcmf board_rotation = get_rot_matrix(board_rotation_id);
matrix::Dcmf board_rotation_t = board_rotation.transpose();
bool tc_locked[3] = {false}; // true when the thermal parameter instance has already been adjusted by the calibrator
for (unsigned uorb_index = 0; uorb_index < active_sensors; uorb_index++) {
/* handle individual sensors, one by one */
matrix::Vector3f accel_offs_vec(accel_offs[uorb_index]);
matrix::Vector3f accel_offs_rotated = board_rotation_t * accel_offs_vec;
matrix::Matrix3f accel_T_mat(accel_T[uorb_index]);
matrix::Matrix3f accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
accel_scale.x_offset = accel_offs_rotated(0);
accel_scale.x_scale = accel_T_rotated(0, 0);
accel_scale.y_offset = accel_offs_rotated(1);
accel_scale.y_scale = accel_T_rotated(1, 1);
accel_scale.z_offset = accel_offs_rotated(2);
accel_scale.z_scale = accel_T_rotated(2, 2);
bool failed = false;
failed = failed || (PX4_OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary)));
PX4_INFO("found offset %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
(double)accel_scale.x_offset,
(double)accel_scale.y_offset,
(double)accel_scale.z_offset);
PX4_INFO("found scale %d: x: %.6f, y: %.6f, z: %.6f", uorb_index,
(double)accel_scale.x_scale,
(double)accel_scale.y_scale,
(double)accel_scale.z_scale);
/* check if thermal compensation is enabled */
int32_t tc_enabled_int;
param_get(param_find("TC_A_ENABLE"), &(tc_enabled_int));
if (tc_enabled_int == 1) {
/* Get struct containing sensor thermal compensation data */
struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */
memset(&sensor_correction, 0, sizeof(sensor_correction));
int sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
orb_copy(ORB_ID(sensor_correction), sensor_correction_sub, &sensor_correction);
orb_unsubscribe(sensor_correction_sub);
/* don't allow a parameter instance to be calibrated more than once by another uORB instance */
if (!tc_locked[sensor_correction.accel_mapping[uorb_index]]) {
tc_locked[sensor_correction.accel_mapping[uorb_index]] = true;
/* update the _X0_ terms to include the additional offset */
int32_t handle;
float val;
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
val = 0.0f;
(void)sprintf(str, "TC_A%u_X0_%u", sensor_correction.accel_mapping[uorb_index], axis_index);
handle = param_find(str);
param_get(handle, &val);
if (axis_index == 0) {
val += accel_scale.x_offset;
} else if (axis_index == 1) {
val += accel_scale.y_offset;
} else if (axis_index == 2) {
val += accel_scale.z_offset;
}
failed |= (PX4_OK != param_set_no_notification(handle, &val));
}
/* update the _SCL_ terms to include the scale factor */
for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
val = 1.0f;
(void)sprintf(str, "TC_A%u_SCL_%u", sensor_correction.accel_mapping[uorb_index], axis_index);
handle = param_find(str);
if (axis_index == 0) {
val = accel_scale.x_scale;
} else if (axis_index == 1) {
val = accel_scale.y_scale;
} else if (axis_index == 2) {
val = accel_scale.z_scale;
}
failed |= (PX4_OK != param_set_no_notification(handle, &val));
}
param_notify_changes();
}
// Ensure the calibration values used by the driver are at default settings when we are using thermal calibration data
accel_scale.x_offset = 0.f;
accel_scale.y_offset = 0.f;
accel_scale.z_offset = 0.f;
accel_scale.x_scale = 1.f;
accel_scale.y_scale = 1.f;
accel_scale.z_scale = 1.f;
}
// save the driver level calibration data
(void)sprintf(str, "CAL_ACC%u_XOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset)));
(void)sprintf(str, "CAL_ACC%u_YOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.y_offset)));
(void)sprintf(str, "CAL_ACC%u_ZOFF", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.z_offset)));
(void)sprintf(str, "CAL_ACC%u_XSCALE", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.x_scale)));
(void)sprintf(str, "CAL_ACC%u_YSCALE", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.y_scale)));
(void)sprintf(str, "CAL_ACC%u_ZSCALE", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(accel_scale.z_scale)));
(void)sprintf(str, "CAL_ACC%u_ID", uorb_index);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(device_id[uorb_index])));
if (failed) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, uorb_index);
return PX4_ERROR;
}
#ifdef __PX4_NUTTX
sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, uorb_index);
fd = px4_open(str, 0);
if (fd < 0) {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "sensor does not exist");
res = PX4_ERROR;
} else {
res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
px4_close(fd);
}
if (res != PX4_OK) {
calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, uorb_index);
}
#endif
}
if (res == PX4_OK) {
/* if there is a any preflight-check system response, let the barrage of messages through */
usleep(200000);
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
} else {
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
}
/* give this message enough time to propagate */
usleep(600000);
return res;
}
int
gps_main(int argc, char *argv[])
{
/* set to default */
const char *device_name = GPS_DEFAULT_UART_PORT;
const char *device_name2 = nullptr;
bool fake_gps = false;
bool enable_sat_info = false;
if (argc < 2) {
goto out;
}
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start")) {
/* work around getopt unreliability */
if (argc > 3) {
if (!strcmp(argv[2], "-d")) {
device_name = argv[3];
} else {
PX4_ERR("DID NOT GET -d");
goto out;
}
}
/* Detect fake gps option */
for (int i = 2; i < argc; i++) {
if (!strcmp(argv[i], "-f")) {
fake_gps = true;
}
}
/* Detect sat info option */
for (int i = 2; i < argc; i++) {
if (!strcmp(argv[i], "-s")) {
enable_sat_info = true;
}
}
/* Allow to use a second gps device */
for (int i = 2; i < argc; i++) {
if (!strcmp(argv[i], "-dualgps")) {
if (argc > i + 1) {
device_name2 = argv[i + 1];
} else {
PX4_ERR("Did not get second device address");
}
}
}
gps::start(device_name, fake_gps, enable_sat_info, 1);
if (device_name2) {
gps::start(device_name2, fake_gps, enable_sat_info, 2);
}
}
if (!strcmp(argv[1], "stop")) {
gps::stop();
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test")) {
gps::test();
}
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset")) {
gps::reset();
}
/*
* Print driver status.
*/
if (!strcmp(argv[1], "status")) {
gps::info();
}
return 0;
out:
PX4_ERR("unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'");
PX4_ERR("[-d " GPS_DEFAULT_UART_PORT "][-f (for enabling fake)][-s (to enable sat info)]");
return 1;
}
int MPU9250::reset_mpu()
{
uint8_t retries;
switch (_whoami) {
case MPU_WHOAMI_9250:
case MPU_WHOAMI_6500:
write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_AUTO);
write_checked_reg(MPUREG_PWR_MGMT_2, 0);
usleep(1000);
break;
}
// Enable I2C bus or Disable I2C bus (recommended on data sheet)
write_checked_reg(MPUREG_USER_CTRL, is_i2c() ? 0 : BIT_I2C_IF_DIS);
// SAMPLE RATE
_set_sample_rate(_sample_rate);
_set_dlpf_filter(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ);
// Gyro scale 2000 deg/s ()
switch (_whoami) {
case MPU_WHOAMI_9250:
case MPU_WHOAMI_6500:
write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
break;
}
// correct gyro scale factors
// scale to rad/s in SI units
// 2000 deg/s = (2000/180)*PI = 34.906585 rad/s
// scaling factor:
// 1/(2^15)*(2000/180)*PI
_gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F);
_gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F;
set_accel_range(ACCEL_RANGE_G);
// INT CFG => Interrupt on Data Ready
write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready
#ifdef USE_I2C
bool bypass = !_mag->is_passthrough();
#else
bool bypass = false;
#endif
/* INT: Clear on any read.
* If this instance is for a device is on I2C bus the Mag will have an i2c interface
* that it will use to access the either: a) the internal mag device on the internal I2C bus
* or b) it could be used to access a downstream I2C devices connected to the chip on
* it's AUX_{ASD|SCL} pins. In either case we need to disconnect (bypass) the internal master
* controller that chip provides as a SPI to I2C bridge.
* so bypass is true if the mag has an i2c non null interfaces.
*/
write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR | (bypass ? BIT_INT_BYPASS_EN : 0));
write_checked_reg(MPUREG_ACCEL_CONFIG2, BITS_ACCEL_CONFIG2_41HZ);
retries = 3;
bool all_ok = false;
while (!all_ok && retries--) {
// Assume all checked values are as expected
all_ok = true;
uint8_t reg;
uint8_t bankcheck = 0;
for (uint8_t i = 0; i < _num_checked_registers; i++) {
if ((reg = read_reg(_checked_registers[i])) != _checked_values[i]) {
write_reg(_checked_registers[i], _checked_values[i]);
PX4_ERR("Reg %d is:%d s/b:%d Tries:%d - bank s/b %d, is %d", _checked_registers[i], reg, _checked_values[i], retries,
REG_BANK(_checked_registers[i]), bankcheck);
all_ok = false;
}
}
}
return all_ok ? OK : -EIO;
}
