void handle_command(struct vehicle_command_s *cmd)
{
/* result of the command */
uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
/* request to set different system mode */
switch (cmd->command) {
case VEHICLE_CMD_PREFLIGHT_STORAGE:
if (((int)(cmd->param3)) == 1) {
/* enable logging */
mavlink_log_info(mavlink_fd, "[log] file:");
mavlink_log_info(mavlink_fd, "logdir");
logging_enabled = true;
}
if (((int)(cmd->param3)) == 0) {
/* disable logging */
mavlink_log_info(mavlink_fd, "[log] stopped.");
logging_enabled = false;
}
break;
default:
/* silently ignore */
break;
}
}
bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error)
{
/* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */
for (size_t i = 0; i < nMissionItems; i++) {
static struct mission_item_s missionitem;
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(dm_current, i, &missionitem, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
if (throw_error) {
return false;
} else {
return true;
}
}
if (home_alt > missionitem.altitude) {
if (throw_error) {
mavlink_log_info(_mavlink_fd, "Waypoint %d below home", i);
return false;
} else {
mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i);
return true;
}
}
}
return true;
}
void
Navigator::on_mission_item_reached()
{
if (myState == NAV_STATE_MISSION) {
_mission.report_mission_item_reached();
if (_do_takeoff) {
/* takeoff completed */
_do_takeoff = false;
mavlink_log_info(_mavlink_fd, "#audio: takeoff completed");
} else {
/* advance by one mission item */
_mission.move_to_next();
}
if (_mission.current_mission_available()) {
set_mission_item();
} else {
/* if no more mission items available then finish mission */
/* loiter at last waypoint */
_reset_loiter_pos = false;
mavlink_log_info(_mavlink_fd, "[navigator] mission completed");
request_loiter_or_ready();
}
} else if (myState == NAV_STATE_RTL) {
/* RTL completed */
if (_rtl_state == RTL_STATE_DESCEND) {
/* hovering above home position, land if needed or loiter */
mavlink_log_info(_mavlink_fd, "[navigator] RTL completed");
if (_mission_item.autocontinue) {
dispatch(EVENT_LAND_REQUESTED);
} else {
_reset_loiter_pos = false;
dispatch(EVENT_LOITER_REQUESTED);
}
} else {
/* next RTL step */
_rtl_state = (RTLState)(_rtl_state + 1);
set_rtl_item();
}
} else if (myState == NAV_STATE_LAND) {
/* landing completed */
mavlink_log_info(_mavlink_fd, "[navigator] landing completed");
dispatch(EVENT_READY_REQUESTED);
}
_mission.publish_mission_result();
}
int do_accel_calibration(int mavlink_fd) {
/* announce change */
mavlink_log_info(mavlink_fd, "accel calibration started");
mavlink_log_info(mavlink_fd, "accel cal progress <0> percent");
/* measure and calculate offsets & scales */
float accel_offs[3];
float accel_scale[3];
int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale);
if (res == OK) {
/* measurements complete successfully, set parameters */
if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0]))
|| param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1]))
|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2]))
|| param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0]))
|| param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1]))
|| param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) {
mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed");
}
int fd = open(ACCEL_DEVICE_PATH, 0);
struct accel_scale ascale = {
accel_offs[0],
accel_scale[0],
accel_offs[1],
accel_scale[1],
accel_offs[2],
accel_scale[2],
};
if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
warn("WARNING: failed to set scale / offsets for accel");
close(fd);
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
}
mavlink_log_info(mavlink_fd, "accel calibration done");
return OK;
} else {
/* measurements error */
mavlink_log_info(mavlink_fd, "accel calibration aborted");
return ERROR;
}
/* exit accel calibration mode */
}
static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, bool report_status)
{
bool success = true; // start with a pass and change to a fail if any test fails
float test_limit = 1.0f; // pass limit re-used for each test
// Get sensor_preflight data if available and exit with a fail recorded if not
int sensors_sub = orb_subscribe(ORB_ID(sensor_preflight));
sensor_preflight_s sensors = {};
if (orb_copy(ORB_ID(sensor_preflight), sensors_sub, &sensors) != PX4_OK) {
goto out;
}
// Use the difference between IMU's to detect a bad calibration.
// If a single IMU is fitted, the value being checked will be zero so this check will always pass.
param_get(param_find("COM_ARM_IMU_ACC"), &test_limit);
if (sensors.accel_inconsistency_m_s_s > test_limit) {
if (report_status) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCELS INCONSISTENT - CHECK CAL");
}
success = false;
goto out;
} else if (sensors.accel_inconsistency_m_s_s > test_limit * 0.8f) {
if (report_status) {
mavlink_log_info(mavlink_log_pub, "PREFLIGHT ADVICE: ACCELS INCONSISTENT - CHECK CAL");
}
}
// Fail if gyro difference greater than 5 deg/sec and notify if greater than 2.5 deg/sec
param_get(param_find("COM_ARM_IMU_GYR"), &test_limit);
if (sensors.gyro_inconsistency_rad_s > test_limit) {
if (report_status) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYROS INCONSISTENT - CHECK CAL");
}
success = false;
goto out;
} else if (sensors.gyro_inconsistency_rad_s > test_limit * 0.5f) {
if (report_status) {
mavlink_log_info(mavlink_log_pub, "PREFLIGHT ADVICE: GYROS INCONSISTENT - CHECK CAL");
}
}
out:
orb_unsubscribe(sensors_sub);
return success;
}
void RunwayTakeoff::update(float airspeed, float alt_agl,
double current_lat, double current_lon, orb_advert_t *mavlink_log_pub)
{
switch (_state) {
case RunwayTakeoffState::THROTTLE_RAMP:
if (hrt_elapsed_time(&_initialized_time) > _throttle_ramp_time) {
_state = RunwayTakeoffState::CLAMPED_TO_RUNWAY;
}
break;
case RunwayTakeoffState::CLAMPED_TO_RUNWAY:
if (airspeed > _airspeed_min.get() * _min_airspeed_scaling.get()) {
_state = RunwayTakeoffState::TAKEOFF;
mavlink_log_info(mavlink_log_pub, "#Takeoff airspeed reached");
}
break;
case RunwayTakeoffState::TAKEOFF:
if (alt_agl > _nav_alt.get()) {
_state = RunwayTakeoffState::CLIMBOUT;
/*
* If we started in heading hold mode, move the navigation start WP to the current location now.
* The navigator will take this as starting point to navigate towards the takeoff WP.
*/
if (_heading_mode.get() == 0) {
_start_wp(0) = (float)current_lat;
_start_wp(1) = (float)current_lon;
}
mavlink_log_info(mavlink_log_pub, "#Climbout");
}
break;
case RunwayTakeoffState::CLIMBOUT:
if (alt_agl > _climbout_diff.get()) {
_climbout = false;
_state = RunwayTakeoffState::FLY;
mavlink_log_info(mavlink_log_pub, "#Navigating to waypoint");
}
break;
default:
break;
}
}
int open_log_file()
{
/* string to hold the path to the log */
char log_file_name[16] = "";
char log_file_path[48] = "";
if (log_name_timestamp && gps_time != 0) {
/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
time_t gps_time_sec = gps_time / 1000000;
struct tm t;
gmtime_r(&gps_time_sec, &t);
strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.bin", &t);
snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name);
} else {
uint16_t file_number = 1; // start with file log001
/* look for the next file that does not exist */
while (file_number <= MAX_NO_LOGFILE) {
/* format log file path: e.g. /fs/microsd/sess001/log001.bin */
snprintf(log_file_name, sizeof(log_file_name), "log%03u.bin", file_number);
snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name);
if (!file_exist(log_file_path)) {
break;
}
file_number++;
}
if (file_number > MAX_NO_LOGFILE) {
/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
warnx("all %d possible files exist already", MAX_NO_LOGFILE);
return -1;
}
}
int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC);
if (fd < 0) {
warn("failed opening log: %s", log_file_name);
mavlink_log_info(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
} else {
warnx("log file: %s", log_file_name);
mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name);
}
return fd;
}
void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp,
double lat, double lon, float gps_alt, float baro_alt)
{
// Reference altitude
if (isfinite(_ekf->states[9])) {
_filter_ref_offset = _ekf->states[9];
} else if (isfinite(-_ekf->hgtMea)) {
_filter_ref_offset = -_ekf->hgtMea;
} else {
_filter_ref_offset = -_baro.altitude;
}
// init filtered gps and baro altitudes
_gps_alt_filt = gps_alt;
_baro_alt_filt = baro_alt;
// Initialize projection
_local_pos.ref_lat = lat;
_local_pos.ref_lon = lon;
_local_pos.ref_alt = gps_alt;
_local_pos.ref_timestamp = timestamp;
map_projection_init(&_pos_ref, lat, lon);
mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
}
int stop_swarm( struct quad_mode_s *state, struct quad_mode_s *mode, orb_advert_t *mode_pub, int *state_sub ) {
if ( state->current_state == (enum QUAD_STATE)QUAD_STATE_SWARMING ) {
mode->cmd = (enum QUAD_CMD)QUAD_CMD_STOP_SWARM;
orb_publish(ORB_ID(quad_mode), *mode_pub, mode);
orb_copy(ORB_ID(quad_mode), *state_sub, state);
struct pollfd fd_state;
fd_state.fd = *state_sub;
fd_state.events = POLLIN;
int ret_state = poll(&fd_state, 1, time_out);
if ( ret_state < 0 ) {
warnx("poll cmd error");
} else if ( ret_state == 0 ) {
return -1;
} else if ( fd_state.revents & POLLIN ) {
orb_copy(ORB_ID(quad_mode), *state_sub, state);
if ( state->current_state == (enum QUAD_STATE)QUAD_STATE_HOVERING )
return 0;
return -3;
} else {
mavlink_log_info(mavlink_fd, "[QC%i] something is very wrong", system_id);
}
} else {
return -2;
}
}
bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing,
dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued,
float default_acceptance_rad,
bool condition_landed)
{
bool failed = false;
bool warned = false;
/* Init if not done yet */
init();
_mavlink_fd = mavlink_fd;
// first check if we have a valid position
if (!home_valid /* can later use global / local pos for finer granularity */) {
failed = true;
warned = true;
mavlink_log_info(_mavlink_fd, "Not yet ready for mission, no position lock.");
} else {
failed = failed || !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued);
}
// check if all mission item commands are supported
failed = failed || !checkMissionItemValidity(dm_current, nMissionItems, condition_landed);
failed = failed || !checkGeofence(dm_current, nMissionItems, geofence);
failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned);
if (isRotarywing) {
failed = failed || !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid, default_acceptance_rad);
} else {
failed = failed || !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid);
}
return !failed;
}
void
RTL::on_activation()
{
/* decide where to enter the RTL procedure when we switch into it */
if (_rtl_state == RTL_STATE_NONE) {
/* for safety reasons don't go into RTL if landed */
if (_navigator->get_vstatus()->condition_landed) {
_rtl_state = RTL_STATE_LANDED;
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
/* if lower than return altitude, climb up first */
} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
+ _param_return_alt.get()) {
_rtl_state = RTL_STATE_CLIMB;
/* otherwise go straight to return */
} else {
/* set altitude setpoint to current altitude */
_rtl_state = RTL_STATE_RETURN;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_global_position()->alt;
}
}
set_rtl_item();
}
void sdlog2_stop_log()
{
warnx("stop logging");
mavlink_log_info(mavlink_fd, "[sdlog2] stop logging");
logging_enabled = false;
/* wake up write thread one last time */
pthread_mutex_lock(&logbuffer_mutex);
logwriter_should_exit = true;
pthread_cond_signal(&logbuffer_cond);
/* unlock, now the writer thread may return */
pthread_mutex_unlock(&logbuffer_mutex);
/* wait for write thread to return */
int ret;
if ((ret = pthread_join(logwriter_pthread, NULL)) != 0) {
warnx("error joining logwriter thread: %i", ret);
}
logwriter_pthread = 0;
pthread_attr_destroy(&logwriter_attr);
sdlog2_status();
}
void sdlog2_start_log()
{
warnx("start logging.");
mavlink_log_info(mavlink_fd, "[sdlog2] start logging");
/* initialize statistics counter */
log_bytes_written = 0;
start_time = hrt_absolute_time();
log_msgs_written = 0;
log_msgs_skipped = 0;
/* initialize log buffer emptying thread */
pthread_attr_t receiveloop_attr;
pthread_attr_init(&receiveloop_attr);
struct sched_param param;
/* low priority, as this is expensive disk I/O */
param.sched_priority = SCHED_PRIORITY_DEFAULT - 40;
(void)pthread_attr_setschedparam(&receiveloop_attr, ¶m);
pthread_attr_setstacksize(&receiveloop_attr, 2048);
logwriter_should_exit = false;
/* start log buffer emptying thread */
if (0 != pthread_create(&logwriter_pthread, &receiveloop_attr, logwriter_thread, &lb)) {
errx(1, "error creating logwriter thread");
}
logging_enabled = true;
// XXX we have to destroy the attr at some point
}
int emergency_land( struct quad_mode_s *state, struct quad_mode_s *mode, orb_advert_t *mode_pub, int *state_sub, struct quad_swarm_cmd_s *swarm_cmd ) {
mode->cmd = (enum QUAD_CMD)QUAD_CMD_LAND;
orb_publish(ORB_ID(quad_mode), *mode_pub, mode);
orb_copy(ORB_ID(quad_mode), *state_sub, state);
struct pollfd fd_state;
fd_state.fd = *state_sub;
fd_state.events = POLLIN;
int ret_state = poll(&fd_state, 1, time_out);
if ( ret_state < 0 ) {
warnx("poll cmd error");
} else if ( ret_state == 0 ) {
return -1;
} else if ( fd_state.revents & POLLIN ) {
orb_copy(ORB_ID(quad_mode), *state_sub, state);
if ( state->current_state == (enum QUAD_STATE)QUAD_STATE_GROUNDED )
return 0;
return -3;
} else {
mavlink_log_info(mavlink_fd, "[QC%i] something is very wrong", system_id);
}
state->error = false;
swarm_cmd->cmd_id = 0;
}
int land( struct quad_mode_s *state, struct quad_mode_s *mode, orb_advert_t *mode_pub, int *state_sub, bool *transition_error ) {
if ( (state->current_state == (enum QUAD_STATE)QUAD_STATE_HOVERING) || *transition_error ) {
mode->cmd = (enum QUAD_CMD)QUAD_CMD_LAND;
orb_publish(ORB_ID(quad_mode), *mode_pub, mode);
orb_copy(ORB_ID(quad_mode), *state_sub, state);
struct pollfd fd_state;
fd_state.fd = *state_sub;
fd_state.events = POLLIN;
int ret_state = poll(&fd_state, 1, time_out);
if ( ret_state < 0 ) {
warnx("poll cmd error");
} else if ( ret_state == 0 ) {
return -1;
} else if ( fd_state.revents & POLLIN ) {
orb_copy(ORB_ID(quad_mode), *state_sub, state);
if ( state->current_state == (enum QUAD_STATE)QUAD_STATE_GROUNDED ) {
return 0;
*transition_error = false;
}
return -3;
} else {
mavlink_log_info(mavlink_fd, "[QC%i] something is very wrong", system_id);
}
} else {
return -2;
}
}
void
MulticopterPositionControl::reset_alt_sp()
{
if (_reset_alt_sp) {
_reset_alt_sp = false;
_pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2);
mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2));
}
}
void
MulticopterPositionControl::reset_alt_sp()
{
if (_reset_alt_sp) {
_reset_alt_sp = false;
_pos_sp(2) = _pos(2);
mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2));
}
}
void sdlog2_status()
{
float kibibytes = log_bytes_written / 1024.0f;
float mebibytes = kibibytes / 1024.0f;
float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f;
warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped);
mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped);
}
int create_log_dir()
{
/* create dir on sdcard if needed */
uint16_t dir_number = 1; // start with dir sess001
int mkdir_ret;
if (log_name_timestamp && gps_time != 0) {
/* use GPS date for log dir naming: e.g. /fs/microsd/2014-01-19 */
time_t gps_time_sec = gps_time / 1000000;
struct tm t;
gmtime_r(&gps_time_sec, &t);
int n = snprintf(log_dir, sizeof(log_dir), "%s/", log_root);
strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &t);
mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO);
if (mkdir_ret == OK) {
warnx("log dir created: %s", log_dir);
} else if (errno != EEXIST) {
warn("failed creating new dir: %s", log_dir);
return -1;
}
} else {
/* look for the next dir that does not exist */
while (dir_number <= MAX_NO_LOGFOLDER) {
/* format log dir: e.g. /fs/microsd/sess001 */
sprintf(log_dir, "%s/sess%03u", log_root, dir_number);
mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO);
if (mkdir_ret == 0) {
warnx("log dir created: %s", log_dir);
break;
} else if (errno != EEXIST) {
warn("failed creating new dir: %s", log_dir);
return -1;
}
/* dir exists already */
dir_number++;
continue;
}
if (dir_number >= MAX_NO_LOGFOLDER) {
/* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */
warnx("all %d possible dirs exist already", MAX_NO_LOGFOLDER);
return -1;
}
}
/* print logging path, important to find log file later */
warnx("log dir: %s", log_dir);
mavlink_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir);
return 0;
}
void
MulticopterPositionControl::reset_pos_sp()
{
if (_reset_pos_sp) {
_reset_pos_sp = false;
_pos_sp(0) = _pos(0);
_pos_sp(1) = _pos(1);
mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1));
}
}
bool
MissionFeasibilityChecker::checkDistanceToFirstWaypoint(const mission_s &mission, float max_distance)
{
if (max_distance <= 0.0f) {
/* param not set, check is ok */
return true;
}
/* find first waypoint (with lat/lon) item in datamanager */
for (size_t i = 0; i < mission.count; i++) {
struct mission_item_s mission_item {};
if (!(dm_read((dm_item_t)mission.dataman_id, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s))) {
/* error reading, mission is invalid */
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Error reading offboard mission.");
return false;
}
/* check only items with valid lat/lon */
if (!MissionBlock::item_contains_position(mission_item)) {
continue;
}
/* check distance from current position to item */
float dist_to_1wp = get_distance_to_next_waypoint(
mission_item.lat, mission_item.lon,
_navigator->get_home_position()->lat, _navigator->get_home_position()->lon);
if (dist_to_1wp < max_distance) {
if (dist_to_1wp > ((max_distance * 3) / 2)) {
/* allow at 2/3 distance, but warn */
mavlink_log_critical(_navigator->get_mavlink_log_pub(),
"First waypoint far away: %d meters.", (int)dist_to_1wp);
_navigator->get_mission_result()->warning = true;
}
return true;
} else {
/* item is too far from home */
mavlink_log_critical(_navigator->get_mavlink_log_pub(),
"First waypoint too far away: %d meters, %d max.",
(int)dist_to_1wp, (int)max_distance);
_navigator->get_mission_result()->warning = true;
return false;
}
}
/* no waypoints found in mission, then we will not fly far away */
return true;
}
void AttitudePositionEstimatorEKF::initializeGPS()
{
// GPS is in scaled integers, convert
double lat = _gps.lat / 1.0e7;
double lon = _gps.lon / 1.0e7;
float gps_alt = _gps.alt / 1e3f;
// Set up height correctly
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
_baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame
// init filtered gps and baro altitudes
_gps_alt_filt = gps_alt;
_baro_alt_filt = _baro.altitude;
_ekf->baroHgt = _baro.altitude;
_ekf->hgtMea = _ekf->baroHgt;
// Set up position variables correctly
_ekf->GPSstatus = _gps.fix_type;
_ekf->gpsLat = math::radians(lat);
_ekf->gpsLon = math::radians(lon) - M_PI;
_ekf->gpsHgt = gps_alt;
// Look up mag declination based on current position
float declination = math::radians(get_mag_declination(lat, lon));
float initVelNED[3];
initVelNED[0] = _gps.vel_n_m_s;
initVelNED[1] = _gps.vel_e_m_s;
initVelNED[2] = _gps.vel_d_m_s;
_ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination);
// Initialize projection
_local_pos.ref_lat = lat;
_local_pos.ref_lon = lon;
_local_pos.ref_alt = gps_alt;
_local_pos.ref_timestamp = _gps.timestamp_position;
map_projection_init(&_pos_ref, lat, lon);
mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
#if 0
warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt,
(double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref,
(double)_baro_ref_offset);
warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv,
(double)math::degrees(declination));
#endif
_gps_initialized = true;
}
bool
MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
float max_distance_to_1st_waypoint, float max_distance_between_waypoints,
bool land_start_req)
{
dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(mission.dataman_id);
const size_t nMissionItems = mission.count;
const bool isRotarywing = (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol);
Geofence &geofence = _navigator->get_geofence();
fw_pos_ctrl_status_s *fw_pos_ctrl_status = _navigator->get_fw_pos_ctrl_status();
const float home_alt = _navigator->get_home_position()->alt;
const bool home_valid = _navigator->home_position_valid();
const bool home_alt_valid = _navigator->home_alt_valid();
bool &warning_issued = _navigator->get_mission_result()->warning;
const float default_acceptance_rad = _navigator->get_default_acceptance_radius();
const float default_altitude_acceptance_rad = _navigator->get_altitude_acceptance_radius();
const bool landed = _navigator->get_land_detected()->landed;
bool failed = false;
bool warned = false;
// first check if we have a valid position
if (!home_alt_valid) {
failed = true;
warned = true;
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Not yet ready for mission, no position lock.");
} else {
failed = failed ||
!checkDistanceToFirstWaypoint(dm_current, nMissionItems, max_distance_to_1st_waypoint, warning_issued);
}
// check if all mission item commands are supported
failed = failed || !checkMissionItemValidity(dm_current, nMissionItems, landed);
failed = failed
|| !checkDistancesBetweenWaypoints(dm_current, nMissionItems, max_distance_between_waypoints, warning_issued);
failed = failed || !checkGeofence(dm_current, nMissionItems, geofence, home_alt, home_valid);
failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_alt_valid, warned);
if (isRotarywing) {
failed = failed
|| !checkRotarywing(dm_current, nMissionItems, home_alt, home_alt_valid, default_altitude_acceptance_rad);
} else {
failed = failed
|| !checkFixedwing(dm_current, nMissionItems, fw_pos_ctrl_status, home_alt, home_alt_valid,
default_acceptance_rad, land_start_req);
}
return !failed;
}
void
EngineFailure::advance_ef()
{
switch (_ef_state) {
case EF_STATE_NONE:
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: Engine failure. Loitering down");
_ef_state = EF_STATE_LOITERDOWN;
break;
default:
break;
}
}
bool
MissionFeasibilityChecker::checkDistancesBetweenWaypoints(const mission_s &mission, float max_distance)
{
if (max_distance <= 0.0f) {
/* param not set, check is ok */
return true;
}
double last_lat = (double)NAN;
double last_lon = (double)NAN;
/* Go through all waypoints */
for (size_t i = 0; i < mission.count; i++) {
struct mission_item_s mission_item {};
if (!(dm_read((dm_item_t)mission.dataman_id, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s))) {
/* error reading, mission is invalid */
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Error reading offboard mission.");
return false;
}
/* check only items with valid lat/lon */
if (!MissionBlock::item_contains_position(mission_item)) {
continue;
}
/* Compare it to last waypoint if already available. */
if (PX4_ISFINITE(last_lat) && PX4_ISFINITE(last_lon)) {
/* check distance from current position to item */
const float dist_between_waypoints = get_distance_to_next_waypoint(
mission_item.lat, mission_item.lon,
last_lat, last_lon);
if (dist_between_waypoints > max_distance) {
/* item is too far from home */
mavlink_log_critical(_navigator->get_mavlink_log_pub(),
"Distance between waypoints too far: %d meters, %d max.",
(int)dist_between_waypoints, (int)max_distance);
_navigator->get_mission_result()->warning = true;
return false;
}
}
last_lat = mission_item.lat;
last_lon = mission_item.lon;
}
/* We ran through all waypoints and have not found any distances between waypoints that are too far. */
return true;
}
int check_if_batt_disconnected(orb_advert_t *mavlink_log_pub) {
struct battery_status_s battery;
memset(&battery,0,sizeof(battery));
int batt_sub = orb_subscribe(ORB_ID(battery_status));
orb_copy(ORB_ID(battery_status), batt_sub, &battery);
if (battery.voltage_filtered_v > 3.0f && !(hrt_absolute_time() - battery.timestamp > 500000)) {
mavlink_log_info(mavlink_log_pub, "Please disconnect battery and try again!");
return ERROR;
}
return OK;
}
void error_msg( int error, bool *transition_error ) {
if ( error == 0 ){
mavlink_log_info(mavlink_fd, "[QC%i] State transition succes!", system_id);
} else if ( error == -1 ) {
mavlink_log_info(mavlink_fd, "[QC%i] No state transition occured!", system_id);
} else if ( error == -2 ) {
mavlink_log_info(mavlink_fd, "[QC%i] Not in correct state!", system_id);
} else if ( error == -3 ) {
mavlink_log_info(mavlink_fd, "[QC%i] Not changed to the correct state!", system_id);
} else {
mavlink_log_info(mavlink_fd, "[QC%i] Unknown return value!", system_id);
}
if ( error == -1 )
*transition_error = true;
}
void
MulticopterPositionControl::reset_pos_sp()
{
if (_reset_pos_sp) {
_reset_pos_sp = false;
/* shift position setpoint to make attitude setpoint continuous */
_pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust / _params.vel_p(0)
- _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
_pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust / _params.vel_p(1)
- _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
}
}
void
Navigator::start_loiter()
{
reset_reached();
_do_takeoff = false;
/* set loiter position if needed */
if (_reset_loiter_pos || !_pos_sp_triplet.current.valid) {
_reset_loiter_pos = false;
_pos_sp_triplet.current.lat = _global_pos.lat;
_pos_sp_triplet.current.lon = _global_pos.lon;
_pos_sp_triplet.current.yaw = NAN; // NAN means to use current yaw
float min_alt_amsl = _parameters.min_altitude + _home_pos.alt;
/* use current altitude if above min altitude set by parameter */
if (_global_pos.alt < min_alt_amsl && !_vstatus.is_rotary_wing) {
_pos_sp_triplet.current.alt = min_alt_amsl;
mavlink_log_info(_mavlink_fd, "#audio: loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
} else {
_pos_sp_triplet.current.alt = _global_pos.alt;
mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude");
}
}
_pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER;
_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
_pos_sp_triplet.current.loiter_direction = 1;
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = true;
_pos_sp_triplet.next.valid = false;
_mission_item_valid = false;
_pos_sp_triplet_updated = true;
}
int do_rc_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, "trim calibration starting");
/* XXX fix this */
// if (current_status.rc_signal) {
// mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal.");
// return;
// }
int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
struct manual_control_setpoint_s sp;
orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
/* set parameters */
float p = sp.roll;
param_set(param_find("TRIM_ROLL"), &p);
p = sp.pitch;
param_set(param_find("TRIM_PITCH"), &p);
p = sp.yaw;
param_set(param_find("TRIM_YAW"), &p);
/* store to permanent storage */
/* auto-save */
int save_ret = param_save_default();
if (save_ret != 0) {
mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed");
close(sub_man);
return ERROR;
}
mavlink_log_info(mavlink_fd, "trim calibration done");
close(sub_man);
return OK;
}
