static void usage()
{
PX4_INFO("usage: navigator {start|stop|status|fencefile}");
}
void OutputMavlink::print_status()
{
PX4_INFO("Output: Mavlink");
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
void
test(enum IST8310_BUS busid)
{
struct ist8310_bus_option &bus = find_bus(busid);
struct mag_report report;
ssize_t sz;
int ret;
const char *path = bus.devpath;
int fd = open(path, O_RDONLY);
if (fd < 0) {
err(1, "%s open failed (try 'ist8310 start')", path);
}
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
err(1, "immediate read failed");
}
print_message(report);
/* check if mag is onboard or external */
if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0) {
errx(1, "failed to get if mag is onboard or external");
}
/* set the queue depth to 5 */
if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) {
errx(1, "failed to set queue depth");
}
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
errx(1, "failed to set 2Hz poll rate");
}
/* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) {
struct pollfd fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = poll(&fds, 1, 2000);
if (ret != 1) {
errx(1, "timed out waiting for sensor data");
}
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
err(1, "periodic read failed");
}
print_message(report);
}
PX4_INFO("PASS");
exit(0);
}
/**
* @brief Starts the driver.
*/
void start(enum LL40LS_BUS busid, uint8_t rotation)
{
int fd, ret;
if (instance) {
PX4_INFO("driver already started");
return;
}
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();
}
}
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 navio_sysfs_rc_in_main(int argc, char *argv[])
{
if (argc < 2) {
usage("missing command");
return 1;
}
if (!strcmp(argv[1], "start")) {
if (rc_input != nullptr && rc_input->isRunning()) {
PX4_WARN("already running");
/* this is not an error */
return 0;
}
rc_input = new RcInput();
// Check if alloc worked.
if (rc_input == nullptr) {
PX4_ERR("alloc failed");
return -1;
}
int ret = rc_input->start();
if (ret != 0) {
PX4_ERR("start failed");
}
return 0;
}
if (!strcmp(argv[1], "stop")) {
if (rc_input == nullptr || !rc_input->isRunning()) {
PX4_WARN("not running");
/* this is not an error */
return 0;
}
rc_input->stop();
// Wait for task to die
int i = 0;
do {
/* wait up to 3s */
usleep(100000);
} while (rc_input->isRunning() && ++i < 30);
delete rc_input;
rc_input = nullptr;
return 0;
}
if (!strcmp(argv[1], "status")) {
if (rc_input != nullptr && rc_input->isRunning()) {
PX4_INFO("running");
} else {
PX4_INFO("not running\n");
}
return 0;
}
usage("unrecognized command");
return 1;
}
int
sf0x_main(int argc, char *argv[])
{
// check for optional arguments
int ch;
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
int myoptind = 1;
const char *myoptarg = NULL;
while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'R':
rotation = (uint8_t)atoi(myoptarg);
PX4_INFO("Setting distance sensor orientation to %d", (int)rotation);
break;
default:
PX4_WARN("Unknown option!");
}
}
/*
* Start/load the driver.
*/
if (!strcmp(argv[myoptind], "start")) {
if (argc > myoptind + 1) {
sf0x::start(argv[myoptind + 1], rotation);
} else {
sf0x::start(SF0X_DEFAULT_PORT, rotation);
}
}
/*
* Stop the driver
*/
if (!strcmp(argv[myoptind], "stop")) {
sf0x::stop();
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[myoptind], "test")) {
sf0x::test();
}
/*
* Reset the driver.
*/
if (!strcmp(argv[myoptind], "reset")) {
sf0x::reset();
}
/*
* Print driver information.
*/
if (!strcmp(argv[myoptind], "info") || !strcmp(argv[1], "status")) {
sf0x::info();
}
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
}
int ver_main(int argc, char *argv[])
{
/* defaults to an error */
int ret = 1;
/* first check if there are at least 2 params */
if (argc >= 2) {
if (argv[1] != NULL) {
if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) {
if (argc >= 3 && argv[2] != NULL) {
/* compare 3rd parameter with HW_ARCH string, in case of match, return 0 */
ret = strncmp(HW_ARCH, argv[2], strlen(HW_ARCH));
if (ret == 0) {
PX4_INFO("match: %s", HW_ARCH);
}
return ret;
} else {
warn("Not enough arguments, try 'ver hwcmp PX4FMU_V2'");
return 1;
}
}
/* check if we want to show all */
bool show_all = !strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str));
if (show_all || !strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) {
printf("HW arch: %s\n", HW_ARCH);
ret = 0;
}
if (show_all || !strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) {
printf("FW git-hash: %s\n", px4_git_version);
unsigned fwver = version_tag_to_number(px4_git_tag);
unsigned major = (fwver >> (8 * 3)) & 0xFF;
unsigned minor = (fwver >> (8 * 2)) & 0xFF;
unsigned patch = (fwver >> (8 * 1)) & 0xFF;
unsigned type = (fwver >> (8 * 0)) & 0xFF;
printf("FW version: %s (%u.%u.%u %u), %u\n", px4_git_tag, major, minor, patch,
type, fwver);
/* middleware is currently the same thing as firmware, so not printing yet */
printf("OS version: %s (%u)\n", os_git_tag, version_tag_to_number(os_git_tag));
ret = 0;
}
if (show_all || !strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) {
printf("Build datetime: %s %s\n", __DATE__, __TIME__);
ret = 0;
}
if (show_all || !strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) {
printf("Toolchain: %s\n", __VERSION__);
ret = 0;
}
if (show_all || !strncmp(argv[1], mcu_ver_str, sizeof(mcu_ver_str))) {
char rev;
char *revstr;
int chip_version = mcu_version(&rev, &revstr);
if (chip_version < 0) {
printf("UNKNOWN MCU\n");
} else {
printf("MCU: %s, rev. %c\n", revstr, rev);
if (chip_version < MCU_REV_STM32F4_REV_3) {
printf("\nWARNING WARNING WARNING!\n"
"Revision %c has a silicon errata\n"
"This device can only utilize a maximum of 1MB flash safely!\n"
"https://pixhawk.org/help/errata\n\n", rev);
}
}
ret = 0;
}
if (show_all || !strncmp(argv[1], mcu_uid_str, sizeof(mcu_uid_str))) {
uint32_t uid[3];
mcu_unique_id(uid);
printf("UID: %X:%X:%X \n", uid[0], uid[1], uid[2]);
ret = 0;
}
if (ret == 1) {
warn("unknown command.\n");
return 1;
}
} else {
void
Syslink::handle_raw_other(syslink_message_t *sys)
{
// This function doesn't actually do anything
// It is just here to return null responses to most standard messages
crtp_message_t *c = (crtp_message_t *) &sys->length;
if (c->port == CRTP_PORT_LOG) {
PX4_INFO("Log: %d %d", c->channel, c->data[0]);
if (c->channel == 0) { // Table of Contents Access
uint8_t cmd = c->data[0];
if (cmd == 0) { // GET_ITEM
//int id = c->data[1];
memset(&c->data[2], 0, 3);
c->data[2] = 1; // type
c->size = 1 + 5;
send_message(sys);
} else if (cmd == 1) { // GET_INFO
memset(&c->data[1], 0, 7);
c->size = 1 + 8;
send_message(sys);
}
} else if (c->channel == 1) { // Log control
uint8_t cmd = c->data[0];
PX4_INFO("Responding to cmd: %d", cmd);
c->data[2] = 0; // Success
c->size = 3 + 1;
// resend message
send_message(sys);
} else if (c->channel == 2) { // Log data
}
} else if (c->port == CRTP_PORT_MEM) {
if (c->channel == 0) { // Info
int cmd = c->data[0];
if (cmd == 1) { // GET_NBR_OF_MEMS
c->data[1] = 0;
c->size = 2 + 1;
// resend message
send_message(sys);
}
}
} else if (c->port == CRTP_PORT_PARAM) {
if (c->channel == 0) { // TOC Access
// uint8_t msgId = c->data[0];
c->data[1] = 0; // Last parameter (id = 0)
memset(&c->data[2], 0, 10);
c->size = 1 + 8;
send_message(sys);
}
else if (c->channel == 1) { // Param read
// 0 is ok
c->data[1] = 0; // value
c->size = 1 + 2;
send_message(sys);
}
} else {
PX4_INFO("Got raw: %d", c->port);
}
}
void BlockLocalPositionEstimator::gpsInit()
{
// check for good gps signal
uint8_t nSat = _sub_gps.get().satellites_used;
float eph = _sub_gps.get().eph;
float epv = _sub_gps.get().epv;
uint8_t fix_type = _sub_gps.get().fix_type;
if (
nSat < 6 ||
eph > _gps_eph_max.get() ||
epv > _gps_epv_max.get() ||
fix_type < 3
) {
_gpsStats.reset();
return;
}
// measure
Vector<double, n_y_gps> y;
if (gpsMeasure(y) != OK) {
_gpsStats.reset();
return;
}
// if finished
if (_gpsStats.getCount() > REQ_GPS_INIT_COUNT) {
// get mean gps values
double gpsLat = _gpsStats.getMean()(0);
double gpsLon = _gpsStats.getMean()(1);
float gpsAlt = _gpsStats.getMean()(2);
_sensorTimeout &= ~SENSOR_GPS;
_sensorFault &= ~SENSOR_GPS;
_gpsStats.reset();
if (!_receivedGps) {
// this is the first time we have received gps
_receivedGps = true;
// note we subtract X_z which is in down directon so it is
// an addition
_gpsAltOrigin = gpsAlt + _x(X_z);
// find lat, lon of current origin by subtracting x and y
// if not using vision position since vision will
// have it's own origin, not necessarily where vehicle starts
if (!(_fusion.get() & FUSE_VIS_POS)) {
double gpsLatOrigin = 0;
double gpsLonOrigin = 0;
// reproject at current coordinates
map_projection_init(&_map_ref, gpsLat, gpsLon);
// find origin
map_projection_reproject(&_map_ref, -_x(X_x), -_x(X_y), &gpsLatOrigin, &gpsLonOrigin);
// reinit origin
map_projection_init(&_map_ref, gpsLatOrigin, gpsLonOrigin);
// always override alt origin on first GPS to fix
// possible baro offset in global altitude at init
_altOrigin = _gpsAltOrigin;
_altOriginInitialized = true;
}
PX4_INFO("[lpe] gps init "
"lat %6.2f lon %6.2f alt %5.1f m",
gpsLat,
gpsLon,
double(gpsAlt));
}
}
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
void
test()
{
PX4_INFO("PASS");
}
void
Syslink::handle_message(syslink_message_t *msg)
{
hrt_abstime t = hrt_absolute_time();
if (t - _lasttime > 1000000) {
pktrate = _count;
rxrate = _count_in;
txrate = _count_out;
nullrate = _null_count;
_lasttime = t;
_count = 0;
_null_count = 0;
_count_in = 0;
_count_out = 0;
}
_count++;
if (msg->type == SYSLINK_PM_ONOFF_SWITCHOFF) {
// When the power button is hit
} else if (msg->type == SYSLINK_PM_BATTERY_STATE) {
if (msg->length != 9) {
return;
}
uint8_t flags = msg->data[0];
int charging = flags & 1;
int powered = flags & 2;
float vbat; //, iset;
memcpy(&vbat, &msg->data[1], sizeof(float));
//memcpy(&iset, &msg->data[5], sizeof(float));
_battery.updateBatteryStatus(t, vbat, -1, true, true, 0, 0, false, &_battery_status);
// Update battery charge state
if (charging) {
_bstate = BAT_CHARGING;
}
/* With the usb plugged in and battery disconnected, it appears to be charged. The voltage check ensures that a battery is connected */
else if (powered && !charging && _battery_status.voltage_filtered_v > 3.7f) {
_bstate = BAT_CHARGED;
} else {
_bstate = BAT_DISCHARGING;
}
// announce the battery status if needed, just publish else
if (_battery_pub != nullptr) {
orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
} else {
_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
}
} else if (msg->type == SYSLINK_RADIO_RSSI) {
uint8_t rssi = msg->data[0]; // Between 40 and 100 meaning -40 dBm to -100 dBm
_rssi = 140 - rssi * 100 / (100 - 40);
} else if (msg->type == SYSLINK_RADIO_RAW) {
handle_raw(msg);
_lastrxtime = t;
} else if ((msg->type & SYSLINK_GROUP) == SYSLINK_RADIO) {
handle_radio(msg);
} else if ((msg->type & SYSLINK_GROUP) == SYSLINK_OW) {
memcpy(&_memory->msgbuf, msg, sizeof(syslink_message_t));
px4_sem_post(&memory_sem);
} else {
PX4_INFO("GOT %d", msg->type);
}
//Send queued messages
if (!_queue.empty()) {
_queue.get(msg, sizeof(syslink_message_t));
send_message(msg);
}
float p = (t % 500000) / 500000.0f;
/* Use LED_GREEN for charging indicator */
if (_bstate == BAT_CHARGED) {
led_on(LED_GREEN);
} else if (_bstate == BAT_CHARGING && p < 0.25f) {
led_on(LED_GREEN);
} else {
led_off(LED_GREEN);
}
/* Alternate RX/TX LEDS when transfering */
bool rx = t - _lastrxtime < 200000,
tx = t - _lasttxtime < 200000;
if (rx && p < 0.25f) {
led_on(LED_RX);
} else {
led_off(LED_RX);
}
if (tx && p > 0.5f && p > 0.75f) {
led_on(LED_TX);
} else {
led_off(LED_TX);
}
// resend parameters if they haven't been acknowledged
if (_params_ack[0] == 0 && t - _params_update[0] > 10000) {
set_channel(_channel);
} else if (_params_ack[1] == 0 && t - _params_update[1] > 10000) {
set_datarate(_rate);
} else if (_params_ack[2] == 0 && t - _params_update[2] > 10000) {
set_address(_addr);
}
}
int
tfmini_main(int argc, char *argv[])
{
// check for optional arguments
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);
PX4_INFO("Setting distance sensor orientation to %d", (int)rotation);
break;
case 'd':
device_path = myoptarg;
PX4_INFO("Using device path '%s'", device_path);
break;
default:
PX4_WARN("Unknown option!");
}
}
/*
* Start/load the driver.
*/
if (!strcmp(argv[myoptind], "start")) {
if (strcmp(device_path, "") != 0) {
tfmini::start(device_path, rotation);
} else {
PX4_WARN("Please specify device path!");
tfmini::usage();
return PX4_ERROR;
}
}
/*
* Stop the driver
*/
if (!strcmp(argv[myoptind], "stop")) {
tfmini::stop();
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[myoptind], "test")) {
tfmini::test();
}
/*
* Reset the driver.
*/
if (!strcmp(argv[myoptind], "reset")) {
tfmini::reset();
}
/*
* Print driver information.
*/
if (!strcmp(argv[myoptind], "info") || !strcmp(argv[1], "status")) {
tfmini::info();
}
PX4_ERR("unrecognized command, try 'start', 'test', 'reset' or 'info'");
return PX4_ERROR;
}
static int usage()
{
PX4_INFO("usage: camera_trigger {start|stop|status|test}\n");
return 1;
}
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 */
struct stat buffer;
if (stat(GEOFENCE_FILENAME, &buffer) == 0) {
PX4_INFO("Loading geofence from %s", GEOFENCE_FILENAME);
_geofence.loadFromFile(GEOFENCE_FILENAME);
}
/* do subscriptions */
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
_fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
_home_pos_sub = orb_subscribe(ORB_ID(home_position));
_offboard_mission_sub = orb_subscribe(ORB_ID(mission));
_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
_traffic_sub = orb_subscribe(ORB_ID(transponder_report));
/* copy all topics first time */
vehicle_status_update();
vehicle_land_detected_update();
global_position_update();
local_position_update();
gps_position_update();
sensor_combined_update();
home_position_update(true);
fw_pos_ctrl_status_update(true);
params_update();
/* wakeup source(s) */
px4_pollfd_struct_t fds[1] = {};
/* Setup of loop */
fds[0].fd = _local_pos_sub;
fds[0].events = POLLIN;
/* rate-limit position subscription to 20 Hz / 50 ms */
orb_set_interval(_local_pos_sub, 50);
while (!_task_should_exit) {
/* wait for up to 1000ms for data */
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);
if (pret == 0) {
/* 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("poll error %d, %d", pret, errno);
usleep(10000);
continue;
} else {
if (fds[0].revents & POLLIN) {
/* success, local pos is available */
local_position_update();
}
}
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;
}
}
/* global position updated */
orb_check(_global_pos_sub, &updated);
if (updated) {
global_position_update();
if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
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();
}
/* 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();
}
/* vehicle_command updated */
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_GO_AROUND) {
// DO_GO_AROUND is currently handled by the position controller (unacknowledged)
// TODO: move DO_GO_AROUND handling to navigator
publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) {
position_setpoint_triplet_s *rep = get_reposition_triplet();
position_setpoint_triplet_s *curr = get_position_setpoint_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;
rep->current.cruising_speed = get_cruising_speed();
rep->current.cruising_throttle = get_cruising_throttle();
// 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)) {
// Position change with optional altitude change
rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7;
rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7;
if (PX4_ISFINITE(cmd.param7)) {
rep->current.alt = cmd.param7;
} else {
rep->current.alt = get_global_position()->alt;
}
} else if (PX4_ISFINITE(cmd.param7) && curr->current.valid
&& PX4_ISFINITE(curr->current.lat)
&& PX4_ISFINITE(curr->current.lon)) {
// Altitude without position change
rep->current.lat = curr->current.lat;
rep->current.lon = curr->current.lon;
rep->current.alt = cmd.param7;
} else {
// All three set to NaN - hold in current position
rep->current.lat = get_global_position()->lat;
rep->current.lon = get_global_position()->lon;
rep->current.alt = get_global_position()->alt;
}
rep->previous.valid = true;
rep->current.valid = true;
rep->next.valid = false;
// CMD_DO_REPOSITION is acknowledged by commander
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) {
position_setpoint_triplet_s *rep = get_takeoff_triplet();
// store current position as previous position and goal as next
rep->previous.yaw = get_global_position()->yaw;
rep->previous.lat = get_global_position()->lat;
rep->previous.lon = get_global_position()->lon;
rep->previous.alt = get_global_position()->alt;
rep->current.loiter_radius = get_loiter_radius();
rep->current.loiter_direction = 1;
rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
if (home_position_valid()) {
rep->current.yaw = cmd.param4;
rep->previous.valid = true;
} else {
rep->current.yaw = get_local_position()->yaw;
rep->previous.valid = false;
}
if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7;
rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7;
} else {
// If one of them is non-finite, reset both
rep->current.lat = NAN;
rep->current.lon = NAN;
}
rep->current.alt = cmd.param7;
rep->current.valid = true;
rep->next.valid = false;
// CMD_NAV_TAKEOFF is acknowledged by commander
} 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
*/
if (_mission.land_start()) {
vehicle_command_s vcmd = {};
vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START;
vcmd.param1 = _mission.get_land_start_index();
publish_vehicle_cmd(&vcmd);
} else {
PX4_WARN("planned mission landing not available");
}
publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) {
if (_mission_result.valid && PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0)) {
if (!_mission.set_current_offboard_mission_index(cmd.param1)) {
PX4_WARN("CMD_MISSION_START failed");
}
}
// CMD_MISSION_START is acknowledged by commander
} 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();
}
}
// TODO: handle responses for supported DO_CHANGE_SPEED options?
publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI
|| cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_ROI
|| cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION
|| cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET
|| cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE) {
_vroi = {};
switch (cmd.command) {
case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI:
case vehicle_command_s::VEHICLE_CMD_NAV_ROI:
_vroi.mode = cmd.param1;
break;
case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION:
_vroi.mode = vehicle_command_s::VEHICLE_ROI_LOCATION;
break;
case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET:
_vroi.mode = vehicle_command_s::VEHICLE_ROI_WPNEXT;
_vroi.pitchOffset = cmd.param5;
_vroi.rollOffset = cmd.param6;
_vroi.yawOffset = cmd.param7;
break;
case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE:
_vroi.mode = vehicle_command_s::VEHICLE_ROI_NONE;
break;
}
switch (_vroi.mode) {
case vehicle_command_s::VEHICLE_ROI_NONE:
break;
case vehicle_command_s::VEHICLE_ROI_WPNEXT:
// TODO: implement point toward next MISSION
break;
case vehicle_command_s::VEHICLE_ROI_WPINDEX:
_vroi.mission_seq = cmd.param2;
break;
case vehicle_command_s::VEHICLE_ROI_LOCATION:
_vroi.lat = cmd.param5;
_vroi.lon = cmd.param6;
_vroi.alt = cmd.param7;
break;
case vehicle_command_s::VEHICLE_ROI_TARGET:
_vroi.target_seq = cmd.param2;
break;
default:
_vroi.mode = vehicle_command_s::VEHICLE_ROI_NONE;
break;
}
_vroi.timestamp = hrt_absolute_time();
if (_vehicle_roi_pub != nullptr) {
orb_publish(ORB_ID(vehicle_roi), _vehicle_roi_pub, &_vroi);
} else {
_vehicle_roi_pub = orb_advertise(ORB_ID(vehicle_roi), &_vroi);
}
publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
}
}
/* Check for traffic */
check_traffic();
/* 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.check(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos,
home_position_valid());
last_geofence_check = hrt_absolute_time();
have_geofence_position_data = false;
_geofence_result.timestamp = hrt_absolute_time();
_geofence_result.geofence_action = _geofence.getGeofenceAction();
_geofence_result.home_required = _geofence.isHomeRequired();
if (!inside) {
/* inform other apps via the mission result */
_geofence_result.geofence_violated = true;
/* Issue a warning about the geofence violation once */
if (!_geofence_violation_warning_sent) {
mavlink_log_critical(&_mavlink_log_pub, "Geofence violation");
_geofence_violation_warning_sent = true;
}
} else {
/* inform other apps via the mission result */
_geofence_result.geofence_violated = false;
/* Reset the _geofence_violation_warning_sent field */
_geofence_violation_warning_sent = false;
}
publish_geofence_result();
}
/* Do stuff according to navigation state set by commander */
NavigatorMode *navigation_mode_new{nullptr};
switch (_vstatus.nav_state) {
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
_pos_sp_triplet_published_invalid_once = false;
navigation_mode_new = &_mission;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
_pos_sp_triplet_published_invalid_once = false;
navigation_mode_new = &_loiter;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
_pos_sp_triplet_published_invalid_once = false;
navigation_mode_new = &_rcLoss;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
_pos_sp_triplet_published_invalid_once = false;
// if RTL is set to use a mission landing and mission has a planned landing, then use MISSION
if (mission_landing_required() && on_mission_landing()) {
navigation_mode_new = &_mission;
} else {
navigation_mode_new = &_rtl;
}
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
_pos_sp_triplet_published_invalid_once = false;
navigation_mode_new = &_takeoff;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
_pos_sp_triplet_published_invalid_once = false;
navigation_mode_new = &_land;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
_pos_sp_triplet_published_invalid_once = false;
navigation_mode_new = &_precland;
_precland.set_mode(PrecLandMode::Required);
break;
case vehicle_status_s::NAVIGATION_STATE_DESCEND:
_pos_sp_triplet_published_invalid_once = false;
navigation_mode_new = &_land;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
_pos_sp_triplet_published_invalid_once = false;
navigation_mode_new = &_dataLinkLoss;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
_pos_sp_triplet_published_invalid_once = false;
navigation_mode_new = &_engineFailure;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
_pos_sp_triplet_published_invalid_once = false;
navigation_mode_new = &_gpsFailure;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
_pos_sp_triplet_published_invalid_once = false;
navigation_mode_new = &_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_new = nullptr;
_can_loiter_at_sp = false;
break;
}
/* we have a new navigation mode: reset triplet */
if (_navigation_mode != navigation_mode_new) {
reset_triplets();
}
_navigation_mode = navigation_mode_new;
/* 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 we landed and have not received takeoff setpoint then stay in idle */
if (_land_detected.landed &&
!((_vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF)
|| (_vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION))) {
_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
_pos_sp_triplet.current.valid = true;
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.next.valid = false;
}
/* 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;
reset_triplets();
}
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);
}
orb_unsubscribe(_global_pos_sub);
orb_unsubscribe(_local_pos_sub);
orb_unsubscribe(_gps_pos_sub);
orb_unsubscribe(_sensor_combined_sub);
orb_unsubscribe(_fw_pos_ctrl_status_sub);
orb_unsubscribe(_vstatus_sub);
orb_unsubscribe(_land_detected_sub);
orb_unsubscribe(_home_pos_sub);
orb_unsubscribe(_offboard_mission_sub);
orb_unsubscribe(_param_update_sub);
orb_unsubscribe(_vehicle_command_sub);
PX4_INFO("exiting");
_navigator_task = -1;
}
void usage()
{
PX4_INFO("usage: uart_esc start -d /dev/tty-3");
PX4_INFO(" uart_esc stop");
PX4_INFO(" uart_esc status");
}
static int micrortps_start(int argc, char *argv[])
{
if (0 > parse_options(argc, argv)) {
printf("EXITING...\n");
_rtps_task = -1;
return -1;
}
switch (_options.transport) {
case options::eTransports::UART: {
transport_node = new UART_node(_options.device, _options.baudrate, _options.poll_ms);
printf("\nUART transport: device: %s; baudrate: %d; sleep: %dms; poll: %dms\n\n",
_options.device, _options.baudrate, _options.sleep_ms, _options.poll_ms);
}
break;
case options::eTransports::UDP: {
transport_node = new UDP_node(_options.recv_port, _options.send_port);
printf("\nUDP transport: recv port: %u; send port: %u; sleep: %dms\n\n",
_options.recv_port, _options.send_port, _options.sleep_ms);
}
break;
default:
_rtps_task = -1;
printf("EXITING...\n");
return -1;
}
if (0 > transport_node->init()) {
printf("EXITING...\n");
_rtps_task = -1;
return -1;
}
struct timespec begin;
int total_read = 0, loop = 0;
uint32_t received = 0;
micrortps_start_topics(begin, total_read, received, loop);
struct timespec end;
px4_clock_gettime(CLOCK_REALTIME, &end);
double elapsed_secs = double(end.tv_sec - begin.tv_sec) + double(end.tv_nsec - begin.tv_nsec) / double(1000000000);
printf("RECEIVED: %lu messages in %d LOOPS, %d bytes in %.03f seconds - %.02fKB/s\n\n",
(unsigned long)received, loop, total_read, elapsed_secs, (double)total_read / (1000 * elapsed_secs));
delete transport_node;
transport_node = nullptr;
PX4_INFO("exiting");
fflush(stdout);
_rtps_task = -1;
return 0;
}
static int usage()
{
PX4_INFO("usage: camera_feedback {start|stop}\n");
return 1;
}
void InputTest::print_status()
{
PX4_INFO("Input: Test");
}
/**
* The daemon thread.
*/
static int frsky_telemetry_thread_main(int argc, char *argv[])
{
/* Default values for arguments */
char *device_name = "/dev/ttyS6"; /* USART8 */
/* Work around some stupidity in task_create's argv handling */
argc -= 2;
argv += 2;
int ch;
while ((ch = getopt(argc, argv, "d:")) != EOF) {
switch (ch) {
case 'd':
device_name = optarg;
break;
default:
usage();
break;
}
}
/* Open UART assuming D type telemetry */
struct termios uart_config_original;
struct termios uart_config;
const int uart = sPort_open_uart(device_name, &uart_config, &uart_config_original);
if (uart < 0) {
warnx("could not open %s", device_name);
err(1, "could not open %s", device_name);
}
/* poll descriptor */
struct pollfd fds[1];
fds[0].fd = uart;
fds[0].events = POLLIN;
thread_running = true;
/* Main thread loop */
char sbuf[20];
frsky_state = SCANNING;
frsky_state_t baudRate = DTYPE;
while (!thread_should_exit && frsky_state == SCANNING) {
/* 2 byte polling frames indicate SmartPort telemetry
* 11 byte packets indicate D type telemetry
*/
int status = poll(fds, sizeof(fds) / sizeof(fds[0]), 3000);
if (status > 0) {
/* traffic on the port, D type is 11 bytes per frame, SmartPort is only 2
* Wait long enough for 11 bytes at 9600 baud
*/
usleep(12000);
int nbytes = read(uart, &sbuf[0], sizeof(sbuf));
PX4_DEBUG("frsky input: %d bytes: %x %x, speed: %d", nbytes, sbuf[0], sbuf[1], baudRate);
// look for valid header byte
if (nbytes > 10) {
if (baudRate == DTYPE) {
// see if we got a valid D-type hostframe
struct adc_linkquality host_frame;
if (frsky_parse_host((uint8_t *)&sbuf[0], nbytes, &host_frame)) {
frsky_state = baudRate;
break;
}
} else {
// check for alternating S.port start bytes
int index = 0;
while (index < 2 && sbuf[index] != 0x7E) { index++; }
if (index < 2) {
int success = 1;
for (int i = index + 2; i < nbytes; i += 2) {
if (sbuf[i] != 0x7E) { success = 0; break; }
}
if (success) {
frsky_state = baudRate;
break;
}
}
}
}
// alternate between S.port and D-type baud rates
if (baudRate == SPORT) {
PX4_DEBUG("setting baud rate to %d", 9600);
set_uart_speed(uart, &uart_config, B9600);
baudRate = DTYPE;
} else {
PX4_DEBUG("setting baud rate to %d", 57600);
set_uart_speed(uart, &uart_config, B57600);
baudRate = SPORT;
}
// wait a second
usleep(1000000);
// flush buffer
read(uart, &sbuf[0], sizeof(sbuf));
}
}
if (frsky_state == SPORT) {
/* Subscribe to topics */
if (!sPort_init()) {
err(1, "could not allocate memory");
}
PX4_INFO("sending FrSky SmartPort telemetry");
struct sensor_baro_s *sensor_baro = malloc(sizeof(struct sensor_baro_s));
if (sensor_baro == NULL) {
err(1, "could not allocate memory");
}
static float filtered_alt = NAN;
int sensor_sub = orb_subscribe(ORB_ID(sensor_baro));
/* send S.port telemetry */
while (!thread_should_exit) {
/* wait for poll frame starting with value 0x7E
* note that only the bus master is supposed to put a 0x7E on the bus.
* slaves use byte stuffing to send 0x7E and 0x7D.
* we expect a poll frame every 12msec
*/
int status = poll(fds, sizeof(fds) / sizeof(fds[0]), 50);
if (status < 1) { continue; }
// read 1 byte
int newBytes = read(uart, &sbuf[0], 1);
if (newBytes < 1 || sbuf[0] != 0x7E) { continue; }
/* wait for ID byte */
status = poll(fds, sizeof(fds) / sizeof(fds[0]), 50);
if (status < 1) { continue; }
hrt_abstime now = hrt_absolute_time();
newBytes = read(uart, &sbuf[1], 1);
/* get a local copy of the current sensor values
* in order to apply a lowpass filter to baro pressure.
*/
static float last_baro_alt = 0;
bool sensor_updated;
orb_check(sensor_sub, &sensor_updated);
if (sensor_updated) {
orb_copy(ORB_ID(sensor_baro), sensor_sub, sensor_baro);
if (isnan(filtered_alt)) {
filtered_alt = sensor_baro->altitude;
} else {
filtered_alt = .05f * sensor_baro->altitude + .95f * filtered_alt;
}
}
// allow a minimum of 500usec before reply
usleep(500);
sPort_update_topics();
static hrt_abstime lastBATV = 0;
static hrt_abstime lastCUR = 0;
static hrt_abstime lastALT = 0;
static hrt_abstime lastSPD = 0;
static hrt_abstime lastFUEL = 0;
static hrt_abstime lastVSPD = 0;
static hrt_abstime lastGPS = 0;
static hrt_abstime lastNAV_STATE = 0;
static hrt_abstime lastGPS_FIX = 0;
switch (sbuf[1]) {
case SMARTPORT_POLL_1:
/* report BATV at 1Hz */
if (now - lastBATV > 1000 * 1000) {
lastBATV = now;
/* send battery voltage */
sPort_send_BATV(uart);
}
break;
case SMARTPORT_POLL_2:
/* report battery current at 5Hz */
if (now - lastCUR > 200 * 1000) {
lastCUR = now;
/* send battery current */
sPort_send_CUR(uart);
}
break;
case SMARTPORT_POLL_3:
/* report altitude at 5Hz */
if (now - lastALT > 200 * 1000) {
lastALT = now;
/* send altitude */
sPort_send_ALT(uart);
}
break;
case SMARTPORT_POLL_4:
/* report speed at 5Hz */
if (now - lastSPD > 200 * 1000) {
lastSPD = now;
/* send speed */
sPort_send_SPD(uart);
}
break;
case SMARTPORT_POLL_5:
/* report fuel at 1Hz */
if (now - lastFUEL > 1000 * 1000) {
lastFUEL = now;
/* send fuel */
sPort_send_FUEL(uart);
}
break;
case SMARTPORT_POLL_6:
/* report vertical speed at 10Hz */
if (now - lastVSPD > 100 * 1000) {
/* estimate vertical speed using first difference and delta t */
uint64_t dt = now - lastVSPD;
float speed = (filtered_alt - last_baro_alt) / (1e-6f * (float)dt);
/* save current alt and timestamp */
last_baro_alt = filtered_alt;
lastVSPD = now;
sPort_send_VSPD(uart, speed);
}
break;
case SMARTPORT_POLL_7:
/* report GPS data elements at 5*5Hz */
if (now - lastGPS > 100 * 1000) {
static int elementCount = 0;
switch (elementCount) {
case 0:
sPort_send_GPS_LON(uart);
elementCount++;
break;
case 1:
sPort_send_GPS_LAT(uart);
elementCount++;
break;
case 2:
sPort_send_GPS_CRS(uart);
elementCount++;
break;
case 3:
sPort_send_GPS_ALT(uart);
elementCount++;
break;
case 4:
sPort_send_GPS_SPD(uart);
elementCount++;
break;
case 5:
sPort_send_GPS_TIME(uart);
elementCount = 0;
break;
}
}
case SMARTPORT_POLL_8:
/* report nav_state as DIY_NAVSTATE 2Hz */
if (now - lastNAV_STATE > 500 * 1000) {
lastNAV_STATE = now;
/* send T1 */
sPort_send_NAV_STATE(uart);
}
/* report satcount and fix as DIY_GPSFIX at 2Hz */
else if (now - lastGPS_FIX > 500 * 1000) {
lastGPS_FIX = now;
/* send T2 */
sPort_send_GPS_FIX(uart);
}
break;
}
}
PX4_DEBUG("freeing sPort memory");
sPort_deinit();
free(sensor_baro);
/* either no traffic on the port (0=>timeout), or D type packet */
} else if (frsky_state == DTYPE) {
/* detected D type telemetry: reconfigure UART */
PX4_INFO("sending FrSky D type telemetry");
int status = set_uart_speed(uart, &uart_config, B9600);
if (status < 0) {
PX4_DEBUG("error setting speed for %s, quitting", device_name);
/* Reset the UART flags to original state */
tcsetattr(uart, TCSANOW, &uart_config_original);
close(uart);
thread_running = false;
return 0;
}
int iteration = 0;
/* Subscribe to topics */
if (!frsky_init()) {
err(1, "could not allocate memory");
}
struct adc_linkquality host_frame;
/* send D8 mode telemetry */
while (!thread_should_exit) {
/* Sleep 100 ms */
usleep(100000);
/* parse incoming data */
int nbytes = read(uart, &sbuf[0], sizeof(sbuf));
bool new_input = frsky_parse_host((uint8_t *)&sbuf[0], nbytes, &host_frame);
/* the RSSI value could be useful */
if (false && new_input) {
warnx("host frame: ad1:%u, ad2: %u, rssi: %u",
host_frame.ad1, host_frame.ad2, host_frame.linkq);
}
frsky_update_topics();
/* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */
if (iteration % 2 == 0) {
frsky_send_frame1(uart);
}
/* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */
if (iteration % 10 == 0) {
frsky_send_frame2(uart);
}
/* Send frame 3 (every 5000ms): date, time */
if (iteration % 50 == 0) {
frsky_send_frame3(uart);
iteration = 0;
}
iteration++;
}
// /* TODO: flush the input buffer if in full duplex mode */
// read(uart, &sbuf[0], sizeof(sbuf));
PX4_DEBUG("freeing frsky memory");
frsky_deinit();
}
/* Reset the UART flags to original state */
tcsetattr(uart, TCSANOW, &uart_config_original);
close(uart);
thread_running = false;
return 0;
}
int
param_main(int argc, char *argv[])
{
if (argc >= 2) {
if (!strcmp(argv[1], "save")) {
if (argc >= 3) {
return do_save(argv[2]);
} else {
int ret = do_save_default();
if (ret) {
PX4_ERR("Param save failed (%i)", ret);
return 1;
} else {
return 0;
}
}
}
if (!strcmp(argv[1], "load")) {
if (argc >= 3) {
return do_load(argv[2]);
} else {
return do_load(param_get_default_file());
}
}
if (!strcmp(argv[1], "import")) {
if (argc >= 3) {
return do_import(argv[2]);
} else {
return do_import(param_get_default_file());
}
}
if (!strcmp(argv[1], "select")) {
if (argc >= 3) {
param_set_default_file(argv[2]);
} else {
param_set_default_file(NULL);
}
PX4_INFO("selected parameter default file %s", param_get_default_file());
return 0;
}
if (!strcmp(argv[1], "show")) {
if (argc >= 3) {
// optional argument -c to show only non-default params
if (!strcmp(argv[2], "-c")) {
if (argc >= 4) {
return do_show(argv[3], true);
} else {
return do_show(NULL, true);
}
} else {
return do_show(argv[2], false);
}
} else {
return do_show(NULL, false);
}
}
if (!strcmp(argv[1], "set")) {
if (argc >= 5) {
/* if the fail switch is provided, fails the command if not found */
bool fail = !strcmp(argv[4], "fail");
return do_set(argv[2], argv[3], fail);
} else if (argc >= 4) {
return do_set(argv[2], argv[3], false);
} else {
PX4_ERR("not enough arguments.\nTry 'param set PARAM_NAME 3 [fail]'");
return 1;
}
}
if (!strcmp(argv[1], "compare")) {
if (argc >= 4) {
return do_compare(argv[2], &argv[3], argc - 3, COMPARE_OPERATOR_EQUAL);
} else {
PX4_ERR("not enough arguments.\nTry 'param compare PARAM_NAME 3'");
return 1;
}
}
if (!strcmp(argv[1], "greater")) {
if (argc >= 4) {
return do_compare(argv[2], &argv[3], argc - 3, COMPARE_OPERATOR_GREATER);
} else {
PX4_ERR("not enough arguments.\nTry 'param greater PARAM_NAME 3'");
return 1;
}
}
if (!strcmp(argv[1], "reset")) {
if (argc >= 3) {
return do_reset((const char **) &argv[2], argc - 2);
} else {
return do_reset(NULL, 0);
}
}
if (!strcmp(argv[1], "reset_nostart")) {
if (argc >= 3) {
return do_reset_nostart((const char **) &argv[2], argc - 2);
} else {
return do_reset_nostart(NULL, 0);
}
}
if (!strcmp(argv[1], "index_used")) {
if (argc >= 3) {
return do_show_index(argv[2], true);
} else {
PX4_ERR("no index provided");
return 1;
}
}
if (!strcmp(argv[1], "index")) {
if (argc >= 3) {
return do_show_index(argv[2], false);
} else {
PX4_ERR("no index provided");
return 1;
}
}
if (!strcmp(argv[1], "find")) {
if (argc >= 3) {
return do_find(argv[2]);
} else {
PX4_ERR("not enough arguments.\nTry 'param find PARAM_NAME'");
return 1;
}
}
}
PX4_INFO("expected a command, try 'load', 'import', 'show [-c] [<filter>]', 'set <param> <value>', 'compare',\n'index', 'index_used', 'find', 'greater', 'select', 'save', or 'reset' ");
return 1;
}
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;
math::Matrix<3, 3> board_rotation;
get_rot_matrix(board_rotation_id, &board_rotation);
math::Matrix<3, 3> board_rotation_t = board_rotation.transposed();
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 */
math::Vector<3> accel_offs_vec(accel_offs[uorb_index]);
math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec;
math::Matrix<3, 3> accel_T_mat(accel_T[uorb_index]);
math::Matrix<3, 3> 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;
}
static int
task_main(int argc, char *argv[])
{
work_q_item_t *work;
/* Initialize global variables */
g_key_offsets[0] = 0;
for (unsigned i = 0; i < (DM_KEY_NUM_KEYS - 1); i++) {
g_key_offsets[i + 1] = g_key_offsets[i] + (g_per_item_max_index[i] * k_sector_size);
}
unsigned max_offset = g_key_offsets[DM_KEY_NUM_KEYS - 1] + (g_per_item_max_index[DM_KEY_NUM_KEYS - 1] * k_sector_size);
for (unsigned i = 0; i < dm_number_of_funcs; i++) {
g_func_counts[i] = 0;
}
/* Initialize the item type locks, for now only DM_KEY_MISSION_STATE supports locking */
px4_sem_init(&g_sys_state_mutex, 1, 1); /* Initially unlocked */
for (unsigned i = 0; i < DM_KEY_NUM_KEYS; i++) {
g_item_locks[i] = NULL;
}
g_item_locks[DM_KEY_MISSION_STATE] = &g_sys_state_mutex;
g_task_should_exit = false;
init_q(&g_work_q);
init_q(&g_free_q);
px4_sem_init(&g_work_queued_sema, 1, 0);
/* See if the data manage file exists and is a multiple of the sector size */
g_task_fd = open(k_data_manager_device_path, O_RDONLY | O_BINARY);
if (g_task_fd >= 0) {
#ifndef __PX4_POSIX
// XXX on Mac OS and Linux the file is not a multiple of the sector sizes
// this might need further inspection.
/* File exists, check its size */
int file_size = lseek(g_task_fd, 0, SEEK_END);
if ((file_size % k_sector_size) != 0) {
PX4_WARN("Incompatible data manager file %s, resetting it", k_data_manager_device_path);
PX4_WARN("Size: %u, sector size: %d", file_size, k_sector_size);
close(g_task_fd);
unlink(k_data_manager_device_path);
}
#else
close(g_task_fd);
#endif
}
/* Open or create the data manager file */
g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY, PX4_O_MODE_666);
if (g_task_fd < 0) {
PX4_WARN("Could not open data manager file %s", k_data_manager_device_path);
px4_sem_post(&g_init_sema); /* Don't want to hang startup */
return -1;
}
if ((unsigned)lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
close(g_task_fd);
PX4_WARN("Could not seek data manager file %s", k_data_manager_device_path);
px4_sem_post(&g_init_sema); /* Don't want to hang startup */
return -1;
}
fsync(g_task_fd);
/* see if we need to erase any items based on restart type */
int sys_restart_val;
const char *restart_type_str = "Unkown restart";
if (param_get(param_find("SYS_RESTART_TYPE"), &sys_restart_val) == OK) {
if (sys_restart_val == DM_INIT_REASON_POWER_ON) {
restart_type_str = "Power on restart";
_restart(DM_INIT_REASON_POWER_ON);
} else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) {
restart_type_str = "In flight restart";
_restart(DM_INIT_REASON_IN_FLIGHT);
}
}
/* We use two file descriptors, one for the caller context and one for the worker thread */
/* They are actually the same but we need to some way to reject caller request while the */
/* worker thread is shutting down but still processing requests */
g_fd = g_task_fd;
PX4_INFO("%s, data manager file '%s' size is %d bytes",
restart_type_str, k_data_manager_device_path, max_offset);
/* Tell startup that the worker thread has completed its initialization */
px4_sem_post(&g_init_sema);
/* Start the endless loop, waiting for then processing work requests */
while (true) {
/* do we need to exit ??? */
if ((g_task_should_exit) && (g_fd >= 0)) {
/* Close the file handle to stop further queuing */
g_fd = -1;
}
if (!g_task_should_exit) {
/* wait for work */
px4_sem_wait(&g_work_queued_sema);
}
/* Empty the work queue */
while ((work = dequeue_work_item())) {
/* handle each work item with the appropriate handler */
switch (work->func) {
case dm_write_func:
g_func_counts[dm_write_func]++;
work->result =
_write(work->write_params.item, work->write_params.index, work->write_params.persistence, work->write_params.buf,
work->write_params.count);
break;
case dm_read_func:
g_func_counts[dm_read_func]++;
work->result =
_read(work->read_params.item, work->read_params.index, work->read_params.buf, work->read_params.count);
break;
case dm_clear_func:
g_func_counts[dm_clear_func]++;
work->result = _clear(work->clear_params.item);
break;
case dm_restart_func:
g_func_counts[dm_restart_func]++;
work->result = _restart(work->restart_params.reason);
break;
default: /* should never happen */
work->result = -1;
break;
}
/* Inform the caller that work is done */
px4_sem_post(&work->wait_sem);
}
/* time to go???? */
if ((g_task_should_exit) && (g_fd < 0)) {
break;
}
}
close(g_task_fd);
g_task_fd = -1;
/* The work queue is now empty, empty the free queue */
for (;;) {
if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL) {
break;
}
if (work->first) {
free(work);
}
}
destroy_q(&g_work_q);
destroy_q(&g_free_q);
px4_sem_destroy(&g_work_queued_sema);
px4_sem_destroy(&g_sys_state_mutex);
return 0;
}
int
ll40ls_main(int argc, char *argv[])
{
int ch;
int myoptind = 1;
const char *myoptarg = nullptr;
enum LL40LS_BUS busid = LL40LS_BUS_I2C_ALL;
uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
while ((ch = px4_getopt(argc, argv, "IXR:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
#ifdef PX4_I2C_BUS_ONBOARD
case 'I':
busid = LL40LS_BUS_I2C_INTERNAL;
break;
#endif
case 'X':
busid = LL40LS_BUS_I2C_EXTERNAL;
break;
case 'R':
rotation = (uint8_t)atoi(myoptarg);
PX4_INFO("Setting Lidar orientation to %d", (int)rotation);
break;
default:
ll40ls::usage();
return 0;
}
}
/* Determine protocol first because it's needed next. */
if (argc > myoptind + 1) {
const char *protocol = argv[myoptind + 1];
if (!strcmp(protocol, "pwm")) {
busid = LL40LS_BUS_PWM;;
} else if (!strcmp(protocol, "i2c")) {
// Do nothing
} else {
warnx("unknown protocol, choose pwm or i2c");
ll40ls::usage();
return 0;
}
}
/* Now determine action. */
if (argc > myoptind) {
const char *verb = argv[myoptind];
if (!strcmp(verb, "start")) {
ll40ls::start(busid, rotation);
} else if (!strcmp(verb, "stop")) {
ll40ls::stop();
} else if (!strcmp(verb, "test")) {
ll40ls::test();
} else if (!strcmp(verb, "reset")) {
ll40ls::reset();
} else if (!strcmp(verb, "regdump")) {
ll40ls::regdump();
} else if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
ll40ls::info();
} else {
ll40ls::usage();
}
return 0;
}
warnx("unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'");
ll40ls::usage();
return 0;
}
static void
usage(void)
{
PX4_INFO("usage: dataman {start [-f datafile]|stop|status|poweronrestart|inflightrestart}");
}
/**
* 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 ret;
int fd = px4_open(SF1XX_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
PX4_ERR("%s open failed (try 'sf1xx start' if the driver is not running)", SF1XX_DEVICE_PATH);
return PX4_ERROR;
}
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
PX4_ERR("immediate read failed");
return PX4_ERROR;
}
print_message(report);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
PX4_ERR("failed to set 2Hz poll rate");
return PX4_ERROR;
}
/* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) {
struct pollfd fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = poll(&fds, 1, 2000);
if (ret != 1) {
PX4_ERR("timed out waiting for sensor data");
return PX4_ERROR;
}
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
PX4_ERR("periodic read failed");
return PX4_ERROR;
}
print_message(report);
}
/* reset the sensor polling to default rate */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
PX4_ERR("failed to set default poll rate");
return PX4_ERROR;
}
px4_close(fd);
PX4_INFO("PASS");
return PX4_OK;
}
int
dataman_main(int argc, char *argv[])
{
if (argc < 2) {
usage();
return -1;
}
if (!strcmp(argv[1], "start")) {
if (g_fd >= 0) {
PX4_WARN("dataman already running");
return -1;
}
if (argc == 4 && strcmp(argv[2], "-f") == 0) {
k_data_manager_device_path = strdup(argv[3]);
PX4_INFO("dataman file set to: %s", k_data_manager_device_path);
} else {
k_data_manager_device_path = strdup(default_device_path);
}
start();
if (g_fd < 0) {
PX4_ERR("dataman start failed");
free(k_data_manager_device_path);
k_data_manager_device_path = NULL;
return -1;
}
return 0;
}
/* Worker thread should be running for all other commands */
if (g_fd < 0) {
PX4_WARN("dataman worker thread not running");
usage();
return -1;
}
if (!strcmp(argv[1], "stop")) {
stop();
free(k_data_manager_device_path);
k_data_manager_device_path = NULL;
} else if (!strcmp(argv[1], "status")) {
status();
} else if (!strcmp(argv[1], "poweronrestart")) {
dm_restart(DM_INIT_REASON_POWER_ON);
} else if (!strcmp(argv[1], "inflightrestart")) {
dm_restart(DM_INIT_REASON_IN_FLIGHT);
} else {
usage();
return -1;
}
return 1;
}
void Ekf2Replay::task_main()
{
// formats
const int _k_max_data_size = 1024; // 16x16 bytes
uint8_t data[_k_max_data_size] = {};
const char param_file[] = "./rootfs/replay_params.txt";
// Open log file from which we read data
// TODO Check if file exists
int fd = ::open(_file_name, O_RDONLY);
// create path to write a replay file
char *replay_log_name;
replay_log_name = strtok(_file_name, ".");
char tmp[] = "_replayed.px4log";
char *path_to_replay_log = (char *) malloc(1 + strlen(tmp) + strlen(replay_log_name));
strcpy(path_to_replay_log, ".");
strcat(path_to_replay_log, replay_log_name);
strcat(path_to_replay_log, tmp);
// create path which tells user location of replay file
char tmp2[] = "./build_posix_sitl_replay/src/firmware/posix";
char *replay_file_location = (char *) malloc(1 + strlen(tmp) + strlen(tmp2) + strlen(replay_log_name));
strcat(replay_file_location, tmp2);
strcat(replay_file_location, replay_log_name);
strcat(replay_file_location, tmp);
// open logfile to write
_write_fd = ::open(path_to_replay_log, O_WRONLY | O_CREAT, S_IRWXU);
std::ifstream tmp_file;
tmp_file.open(param_file);
std::string line;
bool set_default_params_in_file = false;
if (tmp_file.is_open() && ! tmp_file.eof()) {
getline(tmp_file, line);
if (line.empty()) {
// the parameter file is empty so write the default values to it
set_default_params_in_file = true;
}
}
tmp_file.close();
std::ofstream myfile(param_file, std::ios::app);
// subscribe to estimator topics
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
_innov_sub = orb_subscribe(ORB_ID(ekf2_innovations));
_lpos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_control_state_sub = orb_subscribe(ORB_ID(control_state));
// we use attitude updates from the estimator for synchronisation
_fds[0].fd = _att_sub;
_fds[0].events = POLLIN;
bool read_first_header = false;
bool set_user_params = false;
PX4_INFO("Replay in progress... \n");
PX4_INFO("Log data will be written to %s\n", replay_file_location);
while (!_task_should_exit) {
_message_counter++;
uint8_t header[3] = {};
if (::read(fd, header, 3) != 3) {
if (!read_first_header) {
PX4_WARN("error reading log file, is the path printed above correct?");
} else {
PX4_INFO("Done!");
}
_task_should_exit = true;
continue;
}
read_first_header = true;
if ((header[0] != HEAD_BYTE1 || header[1] != HEAD_BYTE2)) {
// we assume that the log file is finished here
PX4_WARN("Done!");
_task_should_exit = true;
continue;
}
// write header but only for messages which are not generated by the estimator
if (needToSaveMessage(header[2])) {
writeMessage(_write_fd, &header[0], 3);
}
if (header[2] == LOG_FORMAT_MSG) {
// format message
struct log_format_s f;
if (::read(fd, &f.type, sizeof(f)) != sizeof(f)) {
PRINT_READ_ERROR;
_task_should_exit = true;
continue;
}
writeMessage(_write_fd, &f.type, sizeof(log_format_s));
memcpy(&_formats[f.type], &f, sizeof(f));
} else if (header[2] == LOG_PARM_MSG) {
// parameter message
if (::read(fd, &data[0], sizeof(log_PARM_s)) != sizeof(log_PARM_s)) {
PRINT_READ_ERROR;
_task_should_exit = true;
continue;
}
writeMessage(_write_fd, &data[0], sizeof(log_PARM_s));
// apply the parameters
char param_name[16];
for (unsigned i = 0; i < 16; i++) {
param_name[i] = data[i];
if (data[i] == '\0') {
break;
}
}
float param_data = 0;
memcpy(¶m_data, &data[16], sizeof(float));
param_t handle = param_find(param_name);
param_type_t param_format = param_type(handle);
if (param_format == PARAM_TYPE_INT32) {
int32_t value = 0;
value = (int32_t)param_data;
param_set(handle, (const void *)&value);
} else if (param_format == PARAM_TYPE_FLOAT) {
param_set(handle, (const void *)¶m_data);
}
// if the user param file was empty then we fill it with the ekf2 parameter values from
// the log file
if (set_default_params_in_file) {
if (strncmp(param_name, "EKF2", 4) == 0) {
std::ostringstream os;
double value = (double)param_data;
os << std::string(param_name) << " ";
os << value << "\n";
myfile << os.str();
}
}
} else if (header[2] == LOG_VER_MSG) {
// version message
if (::read(fd, &data[0], sizeof(log_VER_s)) != sizeof(log_VER_s)) {
PRINT_READ_ERROR;
_task_should_exit = true;
continue;
}
writeMessage(_write_fd, &data[0], sizeof(log_VER_s));
} else if (header[2] == LOG_TIME_MSG) {
// time message
if (::read(fd, &data[0], sizeof(log_TIME_s)) != sizeof(log_TIME_s)) {
// assume that this is because we have reached the end of the file
PX4_INFO("Done!");
_task_should_exit = true;
continue;
}
writeMessage(_write_fd, &data[0], sizeof(log_TIME_s));
} else {
// the first time we arrive here we should apply the parameters specified in the user file
// this makes sure they are applied after the parameter values of the log file
if (!set_user_params) {
myfile.close();
setUserParams(param_file);
set_user_params = true;
}
// data message
if (::read(fd, &data[0], _formats[header[2]].length - 3) != _formats[header[2]].length - 3) {
PX4_INFO("Done!");
_task_should_exit = true;
continue;
}
// all messages which we are not getting from the estimator are written
// back into the replay log file
if (needToSaveMessage(header[2])) {
writeMessage(_write_fd, &data[0], _formats[header[2]].length - 3);
}
if (header[2] == LOG_RPL1_MSG && _part1_counter_ref > 0) {
// we have found another imu replay message while we still have one waiting to be published.
// so publish that now
publishAndWaitForEstimator();
}
// set estimator input data
setEstimatorInput(&data[0], header[2]);
// we have read the imu replay message (part 1) and have waited 3 more cycles for other replay message parts
// e.g. flow, gps or range. we know that in case they were written to the log file they should come right after
// the first replay message, therefore, we can kick the estimator now
if (_part1_counter_ref > 0 && _part1_counter_ref < _message_counter - 3) {
publishAndWaitForEstimator();
}
}
}
::close(_write_fd);
::close(fd);
delete ekf2_replay::instance;
ekf2_replay::instance = nullptr;
}
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) {
(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) {
px4_close(cmd_fd);
PX4_ERR("UNLINKING FILE FAILED");
return 1;
}
}
}
fsync(fileno(stdout));
fsync(fileno(stderr));
usleep(20000);
px4_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;
}
int
ist8310_main(int argc, char *argv[])
{
IST8310_BUS i2c_busid = IST8310_BUS_ALL;
int i2c_addr = IST8310_BUS_I2C_ADDR; /* 7bit */
enum Rotation rotation = ROTATION_NONE;
bool calibrate = false;
int myoptind = 1;
int ch;
const char *myoptarg = nullptr;
while ((ch = px4_getopt(argc, argv, "R:Ca:b:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'R':
rotation = (enum Rotation)atoi(myoptarg);
break;
case 'a':
i2c_addr = (int)strtol(myoptarg, NULL, 0);
break;
case 'b':
i2c_busid = (IST8310_BUS)strtol(myoptarg, NULL, 0);
break;
case 'C':
calibrate = true;
break;
default:
ist8310::usage();
exit(0);
}
}
if (myoptind >= argc) {
ist8310::usage();
return 1;
}
const char *verb = argv[myoptind];
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
ist8310::start(i2c_busid, i2c_addr, rotation);
if (i2c_busid == IST8310_BUS_ALL) {
PX4_ERR("calibration only feasible against one bus");
return 1;
} else if (calibrate && (ist8310::calibrate(i2c_busid) != 0)) {
PX4_ERR("calibration failed");
return 1;
}
return 0;
}
/*
* Test the driver/device.
*/
if (!strcmp(verb, "test")) {
ist8310::test(i2c_busid);
}
/*
* Reset the driver.
*/
if (!strcmp(verb, "reset")) {
ist8310::reset(i2c_busid);
}
/*
* Print driver information.
*/
if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
ist8310::info(i2c_busid);
}
/*
* Autocalibrate the scaling
*/
if (!strcmp(verb, "calibrate")) {
if (ist8310::calibrate(i2c_busid) == 0) {
PX4_INFO("calibration successful");
return 0;
} else {
PX4_ERR("calibration failed");
return 1;
}
}
ist8310::usage();
return 1;
}
static int
do_accel(int argc, char *argv[])
{
int fd;
fd = open(argv[1], 0);
if (fd < 0) {
PX4_ERR("open %s failed (%i)", argv[1], errno);
return 1;
} else {
int ret;
if (argc == 3 && !strcmp(argv[0], "sampling")) {
/* set the accel internal sampling rate up to at least i Hz */
ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[2], NULL, 0));
if (ret) {
PX4_ERR("sampling rate could not be set");
return 1;
}
} else if (argc == 3 && !strcmp(argv[0], "rate")) {
/* set the driver to poll at i Hz */
ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0));
if (ret) {
PX4_ERR("pollrate could not be set");
return 1;
}
} else if (argc == 3 && !strcmp(argv[0], "range")) {
/* set the range to i G */
ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[2], NULL, 0));
if (ret) {
PX4_ERR("range could not be set");
return 1;
}
} else {
print_usage();
return 1;
}
int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0);
int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
int range = ioctl(fd, ACCELIOCGRANGE, 0);
int id = ioctl(fd, DEVIOCGDEVICEID, 0);
int32_t calibration_id = 0;
param_get(param_find("CAL_ACC0_ID"), &(calibration_id));
PX4_INFO("accel: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G",
id, calibration_id, srate, prate, range);
close(fd);
}
return 0;
}
