fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-04 08:00:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #1089 from PX4/navigator_rewrite

Browse Source 
Navigator rewrite
This commit is contained in:
Lorenz Meier
2014-06-29 19:10:42 +02:00
parent 9388b04557 5db51bd704
commit 94c69e11ae
47 changed files with 2986 additions and 2591 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/blinkm/blinkm.cpp
+3 -2
View File
@@ -655,13 +655,14 @@ BlinkM::led()
/* indicate main control state */
if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL)
led_color_4 = LED_GREEN;
else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO)
/* TODO: add other Auto modes */
else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO_MISSION)
led_color_4 = LED_BLUE;
else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL)
led_color_4 = LED_YELLOW;
else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL)
led_color_4 = LED_WHITE;
else
else
led_color_4 = LED_OFF;
led_color_5 = led_color_4;
}
src/drivers/gps/gps.cpp
+3 -3
View File
@@ -280,8 +280,8 @@ GPS::task_main()
_report.p_variance_m = 10.0f;
_report.c_variance_rad = 0.1f;
_report.fix_type = 3;
_report.eph_m = 0.9f;
_report.epv_m = 1.8f;
_report.eph = 0.9f;
_report.epv = 1.8f;
_report.timestamp_velocity = hrt_absolute_time();
_report.vel_n_m_s = 0.0f;
_report.vel_e_m_s = 0.0f;
@@ -451,7 +451,7 @@ GPS::print_info()
warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type,
_report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0f);
warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph_m, (double)_report.epv_m);
warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph, (double)_report.epv);
warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
warnx("rate publication:\t%6.2f Hz", (double)_rate);
src/drivers/gps/mtk.cpp
+4 -4
View File
@@ -251,16 +251,16 @@ MTK::handle_message(gps_mtk_packet_t &packet)
_gps_position->lon = 0;
// Indicate this data is not usable and bail out
_gps_position->eph_m = 1000.0f;
_gps_position->epv_m = 1000.0f;
_gps_position->eph = 1000.0f;
_gps_position->epv = 1000.0f;
_gps_position->fix_type = 0;
return;
}
_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
_gps_position->fix_type = packet.fix_type;
_gps_position->eph_m = packet.hdop / 100.0f; // from cm to m
_gps_position->epv_m = _gps_position->eph_m; // unknown in mtk custom mode, so we cheat with eph
_gps_position->eph = packet.hdop / 100.0f; // from cm to m
_gps_position->epv = _gps_position->eph; // unknown in mtk custom mode, so we cheat with eph
_gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s
_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
_gps_position->satellites_visible = packet.satellites;
src/drivers/gps/ubx.cpp
+2 -2
View File
@@ -439,8 +439,8 @@ UBX::handle_message()
_gps_position->lat = packet->lat;
_gps_position->lon = packet->lon;
_gps_position->alt = packet->height_msl;
_gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m
_gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m
_gps_position->eph = (float)packet->hAcc * 1e-3f; // from mm to m
_gps_position->epv = (float)packet->vAcc * 1e-3f; // from mm to m
_gps_position->timestamp_position = hrt_absolute_time();
_rate_count_lat_lon++;
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+4 -4
View File
@@ -342,7 +342,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
if (gps_updated) {
orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps);
if (gps.eph_m < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));
/* update mag declination rotation matrix */
@@ -401,7 +401,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
hrt_abstime vel_t = 0;
bool vel_valid = false;
if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph_m < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) {
if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) {
vel_valid = true;
if (gps_updated) {
vel_t = gps.timestamp_velocity;
@@ -410,7 +410,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
vel(2) = gps.vel_d_m_s;
}
} else if (ekf_params.acc_comp == 2 && gps.eph_m < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) {
} else if (ekf_params.acc_comp == 2 && gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) {
vel_valid = true;
if (global_pos_updated) {
vel_t = global_pos.timestamp;
@@ -477,7 +477,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
parameters_update(&ekf_param_handles, &ekf_params);
/* update mag declination rotation matrix */
if (gps.eph_m < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) {
mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));
} else {
src/modules/commander/commander.cpp
+315 -320
View File
File diff suppressed because it is too large Load Diff
src/modules/commander/commander_params.c
+12 -1
View File
@@ -39,7 +39,7 @@
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
*/
#include <nuttx/config.h>
@@ -84,3 +84,14 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
* @group Battery Calibration
*/
PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
/**
* Datalink loss mode enabled.
*
* Set to 1 to enable actions triggered when the datalink is lost.
*
* @group commander
* @min 0
* @max 1
*/
PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0);
src/modules/commander/px4_custom_mode.h
+1
View File
@@ -25,6 +25,7 @@ enum PX4_CUSTOM_SUB_MODE_AUTO {
PX4_CUSTOM_SUB_MODE_AUTO_MISSION,
PX4_CUSTOM_SUB_MODE_AUTO_RTL,
PX4_CUSTOM_SUB_MODE_AUTO_LAND,
PX4_CUSTOM_SUB_MODE_AUTO_RTGS
};
union px4_custom_mode {
src/modules/commander/state_machine_helper.cpp
+206 -342
View File
@@ -1,8 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -36,6 +34,9 @@
/**
* @file state_machine_helper.cpp
* State machine helper functions implementations
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
*/
#include <stdio.h>
@@ -59,30 +60,20 @@
#include "state_machine_helper.h"
#include "commander_helper.h"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
static bool arming_state_changed = true;
static bool main_state_changed = true;
static bool failsafe_state_changed = true;
// This array defines the arming state transitions. The rows are the new state, and the columns
// are the current state. Using new state and current state you can index into the array which
// will be true for a valid transition or false for a invalid transition. In some cases even
// though the transition is marked as true additional checks must be made. See arming_state_transition
// code for those checks.
static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = {
// INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
{ /* ARMING_STATE_INIT */ true, true, false, false, false, false, false },
{ /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false },
{ /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true },
{ /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false },
{ /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false },
{ /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true },
{ /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI
// INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
{ /* ARMING_STATE_INIT */ true, true, false, false, false, false, false },
{ /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false },
{ /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true },
{ /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false },
{ /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false },
{ /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true },
{ /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI
};
// You can index into the array with an arming_state_t in order to get it's textual representation
@@ -165,7 +156,6 @@ arming_state_transition(struct vehicle_status_s *status, /// current
armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY;
ret = TRANSITION_CHANGED;
status->arming_state = new_arming_state;
arming_state_changed = true;
}
}
@@ -199,69 +189,58 @@ bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safet
}
}
bool
check_arming_state_changed()
{
if (arming_state_changed) {
arming_state_changed = false;
return true;
} else {
return false;
}
}
transition_result_t
main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state)
{
transition_result_t ret = TRANSITION_DENIED;
/* transition may be denied even if requested the same state because conditions may be changed */
/* transition may be denied even if the same state is requested because conditions may have changed */
switch (new_main_state) {
case MAIN_STATE_MANUAL:
ret = TRANSITION_CHANGED;
break;
case MAIN_STATE_ACRO:
ret = TRANSITION_CHANGED;
break;
case MAIN_STATE_ALTCTL:
/* need at minimum altitude estimate */
/* TODO: add this for fixedwing as well */
if (!status->is_rotary_wing ||
(status->condition_local_altitude_valid ||
status->condition_global_position_valid)) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_POSCTL:
/* need at minimum local position estimate */
if (status->condition_local_position_valid ||
status->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_AUTO:
case MAIN_STATE_AUTO_MISSION:
case MAIN_STATE_AUTO_LOITER:
/* need global position estimate */
if (status->condition_global_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_AUTO_RTL:
/* need global position and home position */
if (status->condition_global_position_valid && status->condition_home_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_MAX:
default:
break;
}
if (ret == TRANSITION_CHANGED) {
if (status->main_state != new_main_state) {
status->main_state = new_main_state;
main_state_changed = true;
} else {
ret = TRANSITION_NOT_CHANGED;
}
@@ -270,70 +249,35 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
return ret;
}
bool
check_main_state_changed()
{
if (main_state_changed) {
main_state_changed = false;
return true;
} else {
return false;
}
}
bool
check_failsafe_state_changed()
{
if (failsafe_state_changed) {
failsafe_state_changed = false;
return true;
} else {
return false;
}
}
/**
* Transition from one hil state to another
*/
int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
* Transition from one hil state to another
*/
transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
bool valid_transition = false;
int ret = ERROR;
transition_result_t ret = TRANSITION_DENIED;
if (current_status->hil_state == new_state) {
valid_transition = true;
ret = TRANSITION_NOT_CHANGED;
} else {
switch (new_state) {
case HIL_STATE_OFF:
/* we're in HIL and unexpected things can happen if we disable HIL now */
mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)");
valid_transition = false;
ret = TRANSITION_DENIED;
break;
case HIL_STATE_ON:
if (current_status->arming_state == ARMING_STATE_INIT
|| current_status->arming_state == ARMING_STATE_STANDBY
|| current_status->arming_state == ARMING_STATE_STANDBY_ERROR) {
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
valid_transition = true;
// Disable publication of all attached sensors
/* Disable publication of all attached sensors */
/* list directory */
DIR *d;
d = opendir("/dev");
if (d) {
struct dirent *direntry;
char devname[24];
@@ -388,290 +332,210 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
}
closedir(d);
ret = TRANSITION_CHANGED;
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
} else {
/* failed opening dir */
warnx("FAILED LISTING DEVICE ROOT DIRECTORY");
return 1;
mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY");
ret = TRANSITION_DENIED;
}
} else {
mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed");
ret = TRANSITION_DENIED;
}
break;
default:
warnx("Unknown hil state");
warnx("Unknown HIL state");
break;
}
}
if (valid_transition) {
if (ret == TRANSITION_CHANGED) {
current_status->hil_state = new_state;
current_status->timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
// XXX also set lockdown here
ret = OK;
} else {
mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition");
orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
}
return ret;
}
/**
* Transition from one failsafe state to another
*/
transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state)
* Check failsafe and main status and set navigation status for navigator accordingly
*/
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished)
{
transition_result_t ret = TRANSITION_DENIED;
navigation_state_t nav_state_old = status->nav_state;
/* transition may be denied even if requested the same state because conditions may be changed */
if (status->failsafe_state == FAILSAFE_STATE_TERMINATION) {
/* transitions from TERMINATION to other states not allowed */
if (new_failsafe_state == FAILSAFE_STATE_TERMINATION) {
ret = TRANSITION_NOT_CHANGED;
}
bool armed = (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR);
status->failsafe = false;
} else {
switch (new_failsafe_state) {
case FAILSAFE_STATE_NORMAL:
/* always allowed (except from TERMINATION state) */
ret = TRANSITION_CHANGED;
break;
/* evaluate main state to decide in normal (non-failsafe) mode */
switch (status->main_state) {
case MAIN_STATE_ACRO:
case MAIN_STATE_MANUAL:
case MAIN_STATE_ALTCTL:
case MAIN_STATE_POSCTL:
/* require RC for all manual modes */
if (status->rc_signal_lost && armed) {
status->failsafe = true;
case FAILSAFE_STATE_RTL:
/* global position and home position required for RTL */
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->set_nav_state = NAV_STATE_RTL;
status->set_nav_state_timestamp = hrt_absolute_time();
ret = TRANSITION_CHANGED;
}
break;
case FAILSAFE_STATE_LAND:
/* at least relative altitude estimate required for landing */
if (status->condition_local_altitude_valid || status->condition_global_position_valid) {
status->set_nav_state = NAV_STATE_LAND;
status->set_nav_state_timestamp = hrt_absolute_time();
ret = TRANSITION_CHANGED;
}
break;
case FAILSAFE_STATE_TERMINATION:
/* always allowed */
ret = TRANSITION_CHANGED;
break;
default:
break;
}
if (ret == TRANSITION_CHANGED) {
if (status->failsafe_state != new_failsafe_state) {
status->failsafe_state = new_failsafe_state;
failsafe_state_changed = true;
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
ret = TRANSITION_NOT_CHANGED;
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
} else {
switch (status->main_state) {
case MAIN_STATE_ACRO:
status->nav_state = NAVIGATION_STATE_ACRO;
break;
case MAIN_STATE_MANUAL:
status->nav_state = NAVIGATION_STATE_MANUAL;
break;
case MAIN_STATE_ALTCTL:
status->nav_state = NAVIGATION_STATE_ALTCTL;
break;
case MAIN_STATE_POSCTL:
status->nav_state = NAVIGATION_STATE_POSCTL;
break;
default:
status->nav_state = NAVIGATION_STATE_MANUAL;
break;
}
}
break;
case MAIN_STATE_AUTO_MISSION:
/* go into failsafe
* - if either the datalink is enabled and lost as well as RC is lost
* - if there is no datalink and the mission is finished */
if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) ||
(!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
/* also go into failsafe if just datalink is lost */
} else if (status->data_link_lost && data_link_loss_enabled) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
/* don't bother if RC is lost and mission is not yet finished */
} else if (status->rc_signal_lost) {
/* this mode is ok, we don't need RC for missions */
status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
} else {
/* everything is perfect */
status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
}
break;
case MAIN_STATE_AUTO_LOITER:
/* go into failsafe if datalink and RC is lost */
if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
/* also go into failsafe if just datalink is lost */
} else if (status->data_link_lost && data_link_loss_enabled) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
/* go into failsafe if RC is lost and datalink loss is not set up */
} else if (status->rc_signal_lost && !data_link_loss_enabled) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
/* don't bother if RC is lost if datalink is connected */
} else if (status->rc_signal_lost) {
/* this mode is ok, we don't need RC for loitering */
status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
} else {
/* everything is perfect */
status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
}
break;
case MAIN_STATE_AUTO_RTL:
/* require global position and home */
if ((!status->condition_global_position_valid || !status->condition_home_position_valid)) {
status->failsafe = true;
if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
} else {
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
}
break;
default:
break;
}
return ret;
return status->nav_state != nav_state_old;
}
// /*
// * Wrapper functions (to be used in the commander), all functions assume lock on current_status
// */
// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR
// *
// * START SUBSYSTEM/EMERGENCY FUNCTIONS
// * */
// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
// {
// current_status->onboard_control_sensors_present |= 1 << *subsystem_type;
// current_status->counter++;
// current_status->timestamp = hrt_absolute_time();
// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
// }
// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
// {
// current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type);
// current_status->counter++;
// current_status->timestamp = hrt_absolute_time();
// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
// /* if a subsystem was removed something went completely wrong */
// switch (*subsystem_type) {
// case SUBSYSTEM_TYPE_GYRO:
// //global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_ACC:
// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_MAG:
// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_GPS:
// {
// uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV];
// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
// //global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency(status_pub, current_status);
// }
// }
// break;
// default:
// break;
// }
// }
// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
// {
// current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type;
// current_status->counter++;
// current_status->timestamp = hrt_absolute_time();
// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
// }
// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
// {
// current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type);
// current_status->counter++;
// current_status->timestamp = hrt_absolute_time();
// orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
// /* if a subsystem was disabled something went completely wrong */
// switch (*subsystem_type) {
// case SUBSYSTEM_TYPE_GYRO:
// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_ACC:
// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_MAG:
// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency_always_critical(status_pub, current_status);
// break;
// case SUBSYSTEM_TYPE_GPS:
// {
// uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY);
// state_machine_emergency(status_pub, current_status);
// }
// }
// break;
// default:
// break;
// }
// }
///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
//
//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
//{
// int ret = 1;
//
//// /* Switch on HIL if in standby and not already in HIL mode */
//// if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
//// && !current_status->flag_hil_enabled) {
//// if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) {
//// /* Enable HIL on request */
//// current_status->flag_hil_enabled = true;
//// ret = OK;
//// state_machine_publish(status_pub, current_status, mavlink_fd);
//// publish_armed_status(current_status);
//// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
////
//// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY &&
//// current_status->flag_fmu_armed) {
////
//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!")
////
//// } else {
////
//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.")
//// }
//// }
//
// /* switch manual / auto */
// if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
// update_state_machine_mode_auto(status_pub, current_status, mavlink_fd);
//
// } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) {
// update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd);
//
// } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
// update_state_machine_mode_guided(status_pub, current_status, mavlink_fd);
//
// } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
// update_state_machine_mode_manual(status_pub, current_status, mavlink_fd);
// }
//
// /* vehicle is disarmed, mode requests arming */
// if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
// /* only arm in standby state */
// // XXX REMOVE
// if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
// ret = OK;
// printf("[cmd] arming due to command request\n");
// }
// }
//
// /* vehicle is armed, mode requests disarming */
// if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
// /* only disarm in ground ready */
// if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
// ret = OK;
// printf("[cmd] disarming due to command request\n");
// }
// }
//
// /* NEVER actually switch off HIL without reboot */
// if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
// warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
// mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL");
// ret = ERROR;
// }
//
// return ret;
//}
src/modules/commander/state_machine_helper.h
+4 -14
View File
@@ -56,25 +56,15 @@ typedef enum {
} transition_result_t;
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd);
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
bool check_arming_state_changed();
transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);
bool check_main_state_changed();
transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state);
bool check_navigation_state_changed();
bool check_failsafe_state_changed();
void set_navigation_state_changed();
int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished);
#endif /* STATE_MACHINE_HELPER_H_ */
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+30 -7
View File
@@ -249,6 +249,10 @@ private:
AttPosEKF *_ekf;
float _velocity_xy_filtered;
float _velocity_z_filtered;
float _airspeed_filtered;
/**
* Update our local parameter cache.
*/
@@ -357,7 +361,10 @@ FixedwingEstimator::FixedwingEstimator() :
_accel_valid(false),
_mag_valid(false),
_mavlink_fd(-1),
_ekf(nullptr)
_ekf(nullptr),
_velocity_xy_filtered(0.0f),
_velocity_z_filtered(0.0f),
_airspeed_filtered(0.0f)
{
last_run = hrt_absolute_time();
@@ -1033,7 +1040,7 @@ FixedwingEstimator::task_main()
float initVelNED[3];
if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph_m < _parameters.pos_stddev_threshold && _gps.epv_m < _parameters.pos_stddev_threshold) {
if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) {
initVelNED[0] = _gps.vel_n_m_s;
initVelNED[1] = _gps.vel_e_m_s;
@@ -1073,7 +1080,7 @@ FixedwingEstimator::task_main()
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_gps_offset);
warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m, (double)math::degrees(declination));
warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination));
_gps_initialized = true;
@@ -1287,6 +1294,22 @@ FixedwingEstimator::task_main()
_local_pos.z_global = false;
_local_pos.yaw = _att.yaw;
_velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy);
_velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz);
_airspeed_filtered = 0.95*_airspeed_filtered + + 0.05*_airspeed.true_airspeed_m_s;
/* crude land detector for fixedwing only,
* TODO: adapt so that it works for both, maybe move to another location
*/
if (_velocity_xy_filtered < 5
&& _velocity_z_filtered < 10
&& _airspeed_filtered < 10) {
_local_pos.landed = true;
} else {
_local_pos.landed = false;
}
/* lazily publish the local position only once available */
if (_local_pos_pub > 0) {
/* publish the attitude setpoint */
@@ -1305,8 +1328,8 @@ FixedwingEstimator::task_main()
_global_pos.lat = est_lat;
_global_pos.lon = est_lon;
_global_pos.time_gps_usec = _gps.time_gps_usec;
_global_pos.eph = _gps.eph_m;
_global_pos.epv = _gps.epv_m;
_global_pos.eph = _gps.eph;
_global_pos.epv = _gps.epv;
}
if (_local_pos.v_xy_valid) {
@@ -1326,8 +1349,8 @@ FixedwingEstimator::task_main()
_global_pos.yaw = _local_pos.yaw;
_global_pos.eph = _gps.eph_m;
_global_pos.epv = _gps.epv_m;
_global_pos.eph = _gps.eph;
_global_pos.epv = _gps.epv;
_global_pos.timestamp = _local_pos.timestamp;
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+3 -7
View File
@@ -153,8 +153,6 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */
bool _setpoint_valid; /**< flag if the position control setpoint is valid */
/** manual control states */
float _altctrl_hold_heading; /**< heading the system should hold in altctrl mode */
double _loiter_hold_lat;
@@ -420,7 +418,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
/* states */
_setpoint_valid(false),
_loiter_hold(false),
land_noreturn_horizontal(false),
land_noreturn_vertical(false),
@@ -691,7 +688,6 @@ FixedwingPositionControl::vehicle_setpoint_poll()
if (pos_sp_triplet_updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
_setpoint_valid = true;
}
}
@@ -736,7 +732,7 @@ void
FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet)
{
if (_global_pos_valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) {
if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) {
/* rotate ground speed vector with current attitude */
math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
@@ -826,7 +822,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
// XXX this should only execute if auto AND safety off (actuators active),
// else integrators should be constantly reset.
if (_control_mode.flag_control_position_enabled) {
if (pos_sp_triplet.current.valid) {
if (!_was_pos_control_mode) {
/* reset integrators */
@@ -869,7 +865,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
if (pos_sp_triplet.current.type == SETPOINT_TYPE_NORMAL) {
if (pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION) {
/* waypoint is a plain navigation waypoint */
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
_att_sp.roll_body = _l1_control.nav_roll();
src/modules/gpio_led/gpio_led.c
+1 -1
View File
@@ -264,7 +264,7 @@ void gpio_led_cycle(FAR void *arg)
pattern = 0x2A; // *_*_*_ fast blink (armed, error)
} else if (priv->status.arming_state == ARMING_STATE_ARMED) {
if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && priv->status.failsafe_state == FAILSAFE_STATE_NORMAL) {
if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && !priv->status.failsafe) {
pattern = 0x3f; // ****** solid (armed)
} else {
src/modules/mavlink/mavlink_main.cpp
+13 -1
View File
@@ -915,7 +915,11 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
case MAV_CMD_NAV_TAKEOFF:
mission_item->pitch_min = mavlink_mission_item->param1;
break;
case MAV_CMD_DO_JUMP:
mission_item->do_jump_mission_index = mavlink_mission_item->param1;
mission_item->do_jump_current_count = 0;
mission_item->do_jump_repeat_count = mavlink_mission_item->param2;
break;
default:
mission_item->acceptance_radius = mavlink_mission_item->param2;
mission_item->time_inside = mavlink_mission_item->param1;
@@ -931,6 +935,9 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
// mission_item->index = mavlink_mission_item->seq;
mission_item->origin = ORIGIN_MAVLINK;
/* reset DO_JUMP count */
mission_item->do_jump_current_count = 0;
return OK;
}
@@ -948,6 +955,11 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_
mavlink_mission_item->param1 = mission_item->pitch_min;
break;
case NAV_CMD_DO_JUMP:
mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
mavlink_mission_item->param2 = mission_item->do_jump_repeat_count;
break;
default:
mavlink_mission_item->param2 = mission_item->acceptance_radius;
mavlink_mission_item->param1 = mission_item->time_inside;
src/modules/mavlink/mavlink_messages.cpp
+61 -34
View File
@@ -120,50 +120,77 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
union px4_custom_mode custom_mode;
custom_mode.data = 0;
if (pos_sp_triplet->nav_state == NAV_STATE_NONE) {
/* use main state when navigator is not active */
if (status->main_state == MAIN_STATE_MANUAL) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
switch (status->nav_state) {
case NAVIGATION_STATE_MANUAL:
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
| (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
break;
} else if (status->main_state == MAIN_STATE_ALTCTL) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;
} else if (status->main_state == MAIN_STATE_POSCTL) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
} else if (status->main_state == MAIN_STATE_AUTO) {
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
} else if (status->main_state == MAIN_STATE_ACRO) {
case NAVIGATION_STATE_ACRO:
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
}
break;
} else {
/* use navigation state when navigator is active */
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
case NAVIGATION_STATE_ALTCTL:
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;
break;
if (pos_sp_triplet->nav_state == NAV_STATE_READY) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
case NAVIGATION_STATE_POSCTL:
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
break;
} else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
} else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) {
case NAVIGATION_STATE_AUTO_MISSION:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
break;
} else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) {
case NAVIGATION_STATE_AUTO_LOITER:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
break;
case NAVIGATION_STATE_AUTO_RTL:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
break;
} else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) {
case NAVIGATION_STATE_LAND:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
}
break;
case NAVIGATION_STATE_AUTO_RTGS:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS;
break;
case NAVIGATION_STATE_TERMINATION:
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
break;
}
*mavlink_custom_mode = custom_mode.data;
@@ -612,8 +639,8 @@ protected:
gps.lat,
gps.lon,
gps.alt,
cm_uint16_from_m_float(gps.eph_m),
cm_uint16_from_m_float(gps.epv_m),
cm_uint16_from_m_float(gps.eph),
cm_uint16_from_m_float(gps.epv),
gps.vel_m_s * 100.0f,
_wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
gps.satellites_visible);
src/modules/mavlink/mavlink_receiver.cpp
+43 -3
View File
@@ -106,6 +106,8 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_telemetry_status_pub(-1),
_rc_pub(-1),
_manual_pub(-1),
_telemetry_heartbeat_time(0),
_radio_status_available(false),
_hil_frames(0),
_old_timestamp(0),
_hil_local_proj_inited(0),
@@ -153,6 +155,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_manual_control(msg);
break;
case MAVLINK_MSG_ID_HEARTBEAT:
handle_message_heartbeat(msg);
break;
case MAVLINK_MSG_ID_ENCAPSULATED_DATA:
MavlinkFTP::getServer()->handle_message(_mavlink, msg);
break;
@@ -418,6 +424,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
memset(&tstatus, 0, sizeof(tstatus));
tstatus.timestamp = hrt_absolute_time();
tstatus.heartbeat_time = _telemetry_heartbeat_time;
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
tstatus.rssi = rstatus.rssi;
tstatus.remote_rssi = rstatus.remrssi;
@@ -433,6 +440,9 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
} else {
orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
}
/* this means that heartbeats alone won't be published to the radio status no more */
_radio_status_available = true;
}
void
@@ -458,6 +468,36 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
}
}
void
MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
{
mavlink_heartbeat_t hb;
mavlink_msg_heartbeat_decode(msg, &hb);
/* ignore own heartbeats, accept only heartbeats from GCS */
if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
_telemetry_heartbeat_time = hrt_absolute_time();
/* if no radio status messages arrive, lets at least publish that heartbeats were received */
if (!_radio_status_available) {
struct telemetry_status_s tstatus;
memset(&tstatus, 0, sizeof(tstatus));
tstatus.timestamp = _telemetry_heartbeat_time;
tstatus.heartbeat_time = _telemetry_heartbeat_time;
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
if (_telemetry_status_pub < 0) {
_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
} else {
orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
}
}
}
}
void
MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
{
@@ -674,12 +714,12 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
hil_gps.lat = gps.lat;
hil_gps.lon = gps.lon;
hil_gps.alt = gps.alt;
hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m
hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m
hil_gps.eph = (float)gps.eph * 1e-2f; // from cm to m
hil_gps.epv = (float)gps.epv * 1e-2f; // from cm to m
hil_gps.timestamp_variance = timestamp;
hil_gps.s_variance_m_s = 5.0f;
hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m;
hil_gps.p_variance_m = hil_gps.eph * hil_gps.eph;
hil_gps.timestamp_velocity = timestamp;
hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
src/modules/mavlink/mavlink_receiver.h
+3
View File
@@ -114,6 +114,7 @@ private:
void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
void handle_message_radio_status(mavlink_message_t *msg);
void handle_message_manual_control(mavlink_message_t *msg);
void handle_message_heartbeat(mavlink_message_t *msg);
void handle_message_hil_sensor(mavlink_message_t *msg);
void handle_message_hil_gps(mavlink_message_t *msg);
void handle_message_hil_state_quaternion(mavlink_message_t *msg);
@@ -140,6 +141,8 @@ private:
orb_advert_t _telemetry_status_pub;
orb_advert_t _rc_pub;
orb_advert_t _manual_pub;
hrt_abstime _telemetry_heartbeat_time;
bool _radio_status_available;
int _hil_frames;
uint64_t _old_timestamp;
bool _hil_local_proj_inited;
src/modules/navigator/geofence.cpp
+5 -4
View File
@@ -78,7 +78,7 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle)
{
double lat = vehicle->lat / 1e7d;
double lon = vehicle->lon / 1e7d;
float alt = vehicle->alt;
//float alt = vehicle->alt;
return inside(lat, lon, vehicle->alt);
}
@@ -116,9 +116,9 @@ bool Geofence::inside(double lat, double lon, float altitude)
}
// skip vertex 0 (return point)
if (((temp_vertex_i.lon) >= lon != (temp_vertex_j.lon >= lon)) &&
(lat <= (temp_vertex_j.lat - temp_vertex_i.lat) * (lon - temp_vertex_i.lon) /
(temp_vertex_j.lon - temp_vertex_i.lon) + temp_vertex_i.lat)) {
if (((double)temp_vertex_i.lon >= lon) != ((double)temp_vertex_j.lon >= lon) &&
(lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) /
(double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) {
c = !c;
}
@@ -294,4 +294,5 @@ Geofence::loadFromFile(const char *filename)
int Geofence::clearDm()
{
dm_clear(DM_KEY_FENCE_POINTS);
return OK;
}
src/modules/navigator/loiter.cpp
+78
View File
@@ -0,0 +1,78 @@
/****************************************************************************
*
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file loiter.cpp
*
* Helper class to loiter
*
* @author Julian Oes <julian@oes.ch>
*/
#include <string.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <fcntl.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include "loiter.h"
Loiter::Loiter(Navigator *navigator, const char *name) :
MissionBlock(navigator, name)
{
/* load initial params */
updateParams();
/* initial reset */
on_inactive();
}
Loiter::~Loiter()
{
}
bool
Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
{
/* set loiter item, don't reuse an existing position setpoint */
return set_loiter_item(pos_sp_triplet);
}
void
Loiter::on_inactive()
{
}
src/modules/navigator/loiter.h
+74
View File
@@ -0,0 +1,74 @@
/***************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file loiter.h
*
* Helper class to loiter
*
* @author Julian Oes <julian@oes.ch>
*/
#ifndef NAVIGATOR_LOITER_H
#define NAVIGATOR_LOITER_H
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include "navigator_mode.h"
#include "mission_block.h"
class Loiter : public MissionBlock
{
public:
/**
* Constructor
*/
Loiter(Navigator *navigator, const char *name);
/**
* Destructor
*/
~Loiter();
/**
* This function is called while the mode is inactive
*/
virtual void on_inactive();
/**
* This function is called while the mode is active
*/
virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
};
#endif
src/modules/navigator/mission.cpp
+461
View File
@@ -0,0 +1,461 @@
/****************************************************************************
*
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file navigator_mission.cpp
*
* Helper class to access missions
*
* @author Julian Oes <julian@oes.ch>
*/
#include <sys/types.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <drivers/drv_hrt.h>
#include <dataman/dataman.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include "navigator.h"
#include "mission.h"
Mission::Mission(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
_param_onboard_enabled(this, "ONBOARD_EN"),
_onboard_mission({0}),
_offboard_mission({0}),
_current_onboard_mission_index(-1),
_current_offboard_mission_index(-1),
_mission_result_pub(-1),
_mission_result({0}),
_mission_type(MISSION_TYPE_NONE)
{
/* load initial params */
updateParams();
/* set initial mission items */
on_inactive();
}
Mission::~Mission()
{
}
void
Mission::on_inactive()
{
_first_run = true;
/* check anyway if missions have changed so that feedback to groundstation is given */
bool onboard_updated;
orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
if (onboard_updated) {
update_onboard_mission();
}
bool offboard_updated;
orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
if (offboard_updated) {
update_offboard_mission();
}
}
bool
Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
{
bool updated = false;
/* check if anything has changed */
bool onboard_updated;
orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
if (onboard_updated) {
update_onboard_mission();
}
bool offboard_updated;
orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
if (offboard_updated) {
update_offboard_mission();
}
/* reset mission items if needed */
if (onboard_updated || offboard_updated || _first_run) {
set_mission_items(pos_sp_triplet);
updated = true;
_first_run = false;
}
/* lets check if we reached the current mission item */
if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
advance_mission();
set_mission_items(pos_sp_triplet);
updated = true;
}
return updated;
}
void
Mission::update_onboard_mission()
{
if (orb_copy(ORB_ID(onboard_mission), _navigator->get_onboard_mission_sub(), &_onboard_mission) == OK) {
/* accept the current index set by the onboard mission if it is within bounds */
if (_onboard_mission.current_index >=0
&& _onboard_mission.current_index < (int)_onboard_mission.count) {
_current_onboard_mission_index = _onboard_mission.current_index;
} else {
/* if less WPs available, reset to first WP */
if (_current_onboard_mission_index >= (int)_onboard_mission.count) {
_current_onboard_mission_index = 0;
/* if not initialized, set it to 0 */
} else if (_current_onboard_mission_index < 0) {
_current_onboard_mission_index = 0;
}
/* otherwise, just leave it */
}
} else {
_onboard_mission.count = 0;
_onboard_mission.current_index = 0;
_current_onboard_mission_index = 0;
}
}
void
Mission::update_offboard_mission()
{
if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) {
/* determine current index */
if (_offboard_mission.current_index >= 0
&& _offboard_mission.current_index < (int)_offboard_mission.count) {
_current_offboard_mission_index = _offboard_mission.current_index;
} else {
/* if less WPs available, reset to first WP */
if (_current_offboard_mission_index >= (int)_offboard_mission.count) {
_current_offboard_mission_index = 0;
/* if not initialized, set it to 0 */
} else if (_current_offboard_mission_index < 0) {
_current_offboard_mission_index = 0;
}
/* otherwise, just leave it */
}
/* Check mission feasibility, for now do not handle the return value,
* however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
dm_item_t dm_current;
if (_offboard_mission.dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current,
(size_t)_offboard_mission.count,
_navigator->get_geofence(),
_navigator->get_home_position()->alt);
} else {
_offboard_mission.count = 0;
_offboard_mission.current_index = 0;
_current_offboard_mission_index = 0;
}
report_current_offboard_mission_item();
}
void
Mission::advance_mission()
{
switch (_mission_type) {
case MISSION_TYPE_ONBOARD:
_current_onboard_mission_index++;
break;
case MISSION_TYPE_OFFBOARD:
_current_offboard_mission_index++;
break;
case MISSION_TYPE_NONE:
default:
break;
}
}
void
Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
{
set_previous_pos_setpoint(pos_sp_triplet);
/* try setting onboard mission item */
if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_ONBOARD) {
mavlink_log_info(_navigator->get_mavlink_fd(),
"#audio: onboard mission running");
}
_mission_type = MISSION_TYPE_ONBOARD;
_navigator->set_can_loiter_at_sp(false);
/* try setting offboard mission item */
} else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_OFFBOARD) {
mavlink_log_info(_navigator->get_mavlink_fd(),
"#audio: offboard mission running");
}
_mission_type = MISSION_TYPE_OFFBOARD;
_navigator->set_can_loiter_at_sp(false);
} else {
if (_mission_type != MISSION_TYPE_NONE) {
mavlink_log_info(_navigator->get_mavlink_fd(),
"#audio: mission finished");
} else {
mavlink_log_info(_navigator->get_mavlink_fd(),
"#audio: no mission available");
}
_mission_type = MISSION_TYPE_NONE;
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached);
set_loiter_item(pos_sp_triplet);
reset_mission_item_reached();
report_mission_finished();
}
}
bool
Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
{
/* make sure param is up to date */
updateParams();
if (_param_onboard_enabled.get() > 0 &&
_current_onboard_mission_index >= 0&&
_current_onboard_mission_index < (int)_onboard_mission.count) {
struct mission_item_s new_mission_item;
if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index,
&new_mission_item)) {
/* convert the current mission item and set it valid */
mission_item_to_position_setpoint(&new_mission_item, current_pos_sp);
current_pos_sp->valid = true;
reset_mission_item_reached();
/* TODO: report this somehow */
memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s));
return true;
}
}
return false;
}
bool
Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
{
if (_current_offboard_mission_index >= 0 &&
_current_offboard_mission_index < (int)_offboard_mission.count) {
dm_item_t dm_current;
if (_offboard_mission.dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
struct mission_item_s new_mission_item;
if (read_mission_item(dm_current, true, &_current_offboard_mission_index, &new_mission_item)) {
/* convert the current mission item and set it valid */
mission_item_to_position_setpoint(&new_mission_item, current_pos_sp);
current_pos_sp->valid = true;
reset_mission_item_reached();
report_current_offboard_mission_item();
memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s));
return true;
}
}
return false;
}
void
Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp)
{
int next_temp_mission_index = _onboard_mission.current_index + 1;
/* try if there is a next onboard mission */
if (_onboard_mission.current_index >= 0 &&
next_temp_mission_index < (int)_onboard_mission.count) {
struct mission_item_s new_mission_item;
if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) {
/* convert next mission item to position setpoint */
mission_item_to_position_setpoint(&new_mission_item, next_pos_sp);
next_pos_sp->valid = true;
return;
}
}
/* give up */
next_pos_sp->valid = false;
return;
}
void
Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp)
{
/* try if there is a next offboard mission */
int next_temp_mission_index = _offboard_mission.current_index + 1;
warnx("next index: %d, count; %d", next_temp_mission_index, _offboard_mission.count);
if (_offboard_mission.current_index >= 0 &&
next_temp_mission_index < (int)_offboard_mission.count) {
dm_item_t dm_current;
if (_offboard_mission.dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
struct mission_item_s new_mission_item;
if (read_mission_item(dm_current, false, &next_temp_mission_index, &new_mission_item)) {
/* convert next mission item to position setpoint */
mission_item_to_position_setpoint(&new_mission_item, next_pos_sp);
next_pos_sp->valid = true;
return;
}
}
/* give up */
next_pos_sp->valid = false;
return;
}
bool
Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index,
struct mission_item_s *new_mission_item)
{
/* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
for (int i=0; i<10; i++) {
const ssize_t len = sizeof(struct mission_item_s);
/* read mission item from datamanager */
if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
mavlink_log_critical(_navigator->get_mavlink_fd(),
"#audio: ERROR waypoint could not be read");
return false;
}
/* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */
if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) {
/* do DO_JUMP as many times as requested */
if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) {
/* only raise the repeat count if this is for the current mission item
* but not for the next mission item */
if (is_current) {
(new_mission_item->do_jump_current_count)++;
/* save repeat count */
if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET,
new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the
* dataman */
mavlink_log_critical(_navigator->get_mavlink_fd(),
"#audio: ERROR DO JUMP waypoint could not be written");
return false;
}
}
/* set new mission item index and repeat
* we don't have to validate here, if it's invalid, we should realize this later .*/
*mission_index = new_mission_item->do_jump_mission_index;
} else {
mavlink_log_info(_navigator->get_mavlink_fd(),
"#audio: DO JUMP repetitions completed");
/* no more DO_JUMPS, therefore just try to continue with next mission item */
(*mission_index)++;
}
} else {
/* if it's not a DO_JUMP, then we were successful */
return true;
}
}
/* we have given up, we don't want to cycle forever */
mavlink_log_critical(_navigator->get_mavlink_fd(),
"#audio: ERROR DO JUMP is cycling, giving up");
return false;
}
void
Mission::report_mission_item_reached()
{
if (_mission_type == MISSION_TYPE_OFFBOARD) {
_mission_result.mission_reached = true;
_mission_result.mission_index_reached = _current_offboard_mission_index;
}
publish_mission_result();
}
void
Mission::report_current_offboard_mission_item()
{
_mission_result.index_current_mission = _current_offboard_mission_index;
publish_mission_result();
}
void
Mission::report_mission_finished()
{
_mission_result.mission_finished = true;
publish_mission_result();
}
void
Mission::publish_mission_result()
{
/* lazily publish the mission result only once available */
if (_mission_result_pub > 0) {
/* publish mission result */
orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
} else {
/* advertise and publish */
_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
}
/* reset reached bool */
_mission_result.mission_reached = false;
_mission_result.mission_finished = false;
}
src/modules/navigator/mission.h
+178
View File
@@ -0,0 +1,178 @@
/****************************************************************************
*
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mission.h
*
* Navigator mode to access missions
*
* @author Julian Oes <julian@oes.ch>
*/
#ifndef NAVIGATOR_MISSION_H
#define NAVIGATOR_MISSION_H
#include <drivers/drv_hrt.h>
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <dataman/dataman.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include "navigator_mode.h"
#include "mission_block.h"
#include "mission_feasibility_checker.h"
class Navigator;
class Mission : public MissionBlock
{
public:
/**
* Constructor
*/
Mission(Navigator *navigator, const char *name);
/**
* Destructor
*/
virtual ~Mission();
/**
* This function is called while the mode is inactive
*/
virtual void on_inactive();
/**
* This function is called while the mode is active
*/
virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
private:
/**
* Update onboard mission topic
*/
void update_onboard_mission();
/**
* Update offboard mission topic
*/
void update_offboard_mission();
/**
* Move on to next mission item or switch to loiter
*/
void advance_mission();
/**
* Set new mission items
*/
void set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet);
/**
* Try to set the current position setpoint from an onboard mission item
* @return true if mission item successfully set
*/
bool is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp);
/**
* Try to set the current position setpoint from an offboard mission item
* @return true if mission item successfully set
*/
bool is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp);
/**
* Try to set the next position setpoint from an onboard mission item
*/
void get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp);
/**
* Try to set the next position setpoint from an offboard mission item
*/
void get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp);
/**
* Read a mission item from the dataman and watch out for DO_JUMPS
* @return true if successful
*/
bool read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index,
struct mission_item_s *new_mission_item);
/**
* Report that a mission item has been reached
*/
void report_mission_item_reached();
/**
* Rport the current mission item
*/
void report_current_offboard_mission_item();
/**
* Report that the mission is finished if one exists or that none exists
*/
void report_mission_finished();
/**
* Publish the mission result so commander and mavlink know what is going on
*/
void publish_mission_result();
control::BlockParamFloat _param_onboard_enabled;
struct mission_s _onboard_mission;
struct mission_s _offboard_mission;
int _current_onboard_mission_index;
int _current_offboard_mission_index;
orb_advert_t _mission_result_pub;
struct mission_result_s _mission_result;
enum {
MISSION_TYPE_NONE,
MISSION_TYPE_ONBOARD,
MISSION_TYPE_OFFBOARD
} _mission_type;
MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */
};
#endif
src/modules/navigator/mission_block.cpp
+242
View File
@@ -0,0 +1,242 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mission_block.cpp
*
* Helper class to use mission items
*
* @author Julian Oes <julian@oes.ch>
*/
#include <sys/types.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <uORB/uORB.h>
#include "navigator.h"
#include "mission_block.h"
MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
NavigatorMode(navigator, name),
_waypoint_position_reached(false),
_waypoint_yaw_reached(false),
_time_first_inside_orbit(0),
_mission_item({0}),
_mission_item_valid(false)
{
}
MissionBlock::~MissionBlock()
{
}
bool
MissionBlock::is_mission_item_reached()
{
if (_mission_item.nav_cmd == NAV_CMD_LAND) {
return _navigator->get_vstatus()->condition_landed;
}
/* TODO: count turns */
if ((/*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED)) {
return false;
}
hrt_abstime now = hrt_absolute_time();
if (!_waypoint_position_reached) {
float dist = -1.0f;
float dist_xy = -1.0f;
float dist_z = -1.0f;
float altitude_amsl = _mission_item.altitude_is_relative
? _mission_item.altitude + _navigator->get_home_position()->alt
: _mission_item.altitude;
dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
_navigator->get_global_position()->alt,
&dist_xy, &dist_z);
if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator->get_vstatus()->is_rotary_wing) {
/* require only altitude for takeoff for multicopter */
if (_navigator->get_global_position()->alt >
altitude_amsl - _navigator->get_acceptance_radius()) {
_waypoint_position_reached = true;
}
} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
/* for takeoff mission items use the parameter for the takeoff acceptance radius */
if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) {
_waypoint_position_reached = true;
}
} else {
/* for normal mission items used their acceptance radius */
if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) {
_waypoint_position_reached = true;
}
}
}
if (_waypoint_position_reached && !_waypoint_yaw_reached) {
/* TODO: removed takeoff, why? */
if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
/* check yaw if defined only for rotary wing except takeoff */
float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw);
if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */
_waypoint_yaw_reached = true;
}
} else {
_waypoint_yaw_reached = true;
}
}
/* check if the current waypoint was reached */
if (_waypoint_position_reached && _waypoint_yaw_reached) {
if (_time_first_inside_orbit == 0) {
_time_first_inside_orbit = now;
// if (_mission_item.time_inside > 0.01f) {
// mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs",
// (double)_mission_item.time_inside);
// }
}
/* check if the MAV was long enough inside the waypoint orbit */
if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f) {
return true;
}
}
return false;
}
void
MissionBlock::reset_mission_item_reached()
{
_waypoint_position_reached = false;
_waypoint_yaw_reached = false;
_time_first_inside_orbit = 0;
}
void
MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp)
{
sp->valid = true;
sp->lat = item->lat;
sp->lon = item->lon;
sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude;
sp->yaw = item->yaw;
sp->loiter_radius = item->loiter_radius;
sp->loiter_direction = item->loiter_direction;
sp->pitch_min = item->pitch_min;
switch (item->nav_cmd) {
case NAV_CMD_IDLE:
sp->type = SETPOINT_TYPE_IDLE;
break;
case NAV_CMD_TAKEOFF:
sp->type = SETPOINT_TYPE_TAKEOFF;
break;
case NAV_CMD_LAND:
sp->type = SETPOINT_TYPE_LAND;
break;
case NAV_CMD_LOITER_TIME_LIMIT:
case NAV_CMD_LOITER_TURN_COUNT:
case NAV_CMD_LOITER_UNLIMITED:
sp->type = SETPOINT_TYPE_LOITER;
break;
default:
sp->type = SETPOINT_TYPE_POSITION;
break;
}
}
void
MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet)
{
/* reuse current setpoint as previous setpoint */
if (pos_sp_triplet->current.valid) {
memcpy(&pos_sp_triplet->previous, &pos_sp_triplet->current, sizeof(struct position_setpoint_s));
}
}
bool
MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet)
{
/* don't change setpoint if 'can_loiter_at_sp' flag set */
if (!(_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) {
/* use current position */
pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
pos_sp_triplet->current.yaw = NAN; /* NAN means to use current yaw */
_navigator->set_can_loiter_at_sp(true);
}
if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER
|| pos_sp_triplet->current.loiter_radius != _navigator->get_loiter_radius()
|| pos_sp_triplet->current.loiter_direction != 1
|| pos_sp_triplet->previous.valid
|| !pos_sp_triplet->current.valid
|| pos_sp_triplet->next.valid) {
/* position setpoint triplet should be updated */
pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
pos_sp_triplet->current.loiter_radius = _navigator->get_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;
return true;
}
return false;
}
src/modules/navigator/navigator_mission.h → src/modules/navigator/mission_block.h
+50 -49
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -31,75 +31,76 @@
*
****************************************************************************/
/**
* @file navigator_mission.h
* Helper class to access missions
* @file mission_block.h
*
* @author Julian Oes <joes@student.ethz.ch>
* Helper class to use mission items
*
* @author Julian Oes <julian@oes.ch>
*/
#ifndef NAVIGATOR_MISSION_H
#define NAVIGATOR_MISSION_H
#ifndef NAVIGATOR_MISSION_BLOCK_H
#define NAVIGATOR_MISSION_BLOCK_H
#include <drivers/drv_hrt.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include "navigator_mode.h"
class __EXPORT Mission
class Navigator;
class MissionBlock : public NavigatorMode
{
public:
/**
* Constructor
*/
Mission();
MissionBlock(Navigator *navigator, const char *name);
/**
* Destructor, also kills the sensors task.
* Destructor
*/
~Mission();
virtual ~MissionBlock();
void set_offboard_dataman_id(int new_id);
void set_current_offboard_mission_index(int new_index);
void set_current_onboard_mission_index(int new_index);
void set_offboard_mission_count(unsigned new_count);
void set_onboard_mission_count(unsigned new_count);
/**
* Check if mission item has been reached
* @return true if successfully reached
*/
bool is_mission_item_reached();
/**
* Reset all reached flags
*/
void reset_mission_item_reached();
void set_onboard_mission_allowed(bool allowed);
/**
* Convert a mission item to a position setpoint
*
* @param the mission item to convert
* @param the position setpoint that needs to be set
*/
void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
bool current_mission_available();
bool next_mission_available();
/**
* Set previous position setpoint to current setpoint
*/
void set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet);
int get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, unsigned *index);
int get_next_mission_item(struct mission_item_s *mission_item);
/**
* Set a loiter item, if possible reuse the position setpoint, otherwise take the current position
*
* @param the position setpoint triplet to set
* @return true if setpoint has changed
*/
bool set_loiter_item(position_setpoint_triplet_s *pos_sp_triplet);
void move_to_next();
bool _waypoint_position_reached;
bool _waypoint_yaw_reached;
hrt_abstime _time_first_inside_orbit;
void report_mission_item_reached();
void report_current_offboard_mission_item();
void publish_mission_result();
private:
bool current_onboard_mission_available();
bool current_offboard_mission_available();
bool next_onboard_mission_available();
bool next_offboard_mission_available();
int _offboard_dataman_id;
unsigned _current_offboard_mission_index;
unsigned _current_onboard_mission_index;
unsigned _offboard_mission_item_count; /** number of offboard mission items available */
unsigned _onboard_mission_item_count; /** number of onboard mission items available */
bool _onboard_mission_allowed;
enum {
MISSION_TYPE_NONE,
MISSION_TYPE_ONBOARD,
MISSION_TYPE_OFFBOARD,
} _current_mission_type;
int _mission_result_pub;
struct mission_result_s _mission_result;
mission_item_s _mission_item;
bool _mission_item_valid;
};
#endif
src/modules/navigator/mission_feasibility_checker.cpp
+2 -1
View File
@@ -215,11 +215,12 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
// float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt;
return false;
}
void MissionFeasibilityChecker::updateNavigationCapabilities()
{
int res = orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
(void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
}
void MissionFeasibilityChecker::init()
src/modules/navigator/navigator_state.h → src/modules/navigator/mission_params.c
+30 -16
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -32,24 +32,38 @@
****************************************************************************/
/**
* @file navigator_state.h
* @file mission_params.c
*
* Navigator state
* Parameters for mission.
*
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Julian Oes <joes@student.ethz.ch>
*/
#ifndef NAVIGATOR_STATE_H_
#define NAVIGATOR_STATE_H_
#include <nuttx/config.h>
typedef enum {
NAV_STATE_NONE = 0,
NAV_STATE_READY,
NAV_STATE_LOITER,
NAV_STATE_MISSION,
NAV_STATE_RTL,
NAV_STATE_LAND,
NAV_STATE_MAX
} nav_state_t;
#include <systemlib/param/param.h>
#endif /* NAVIGATOR_STATE_H_ */
/*
* Mission parameters, accessible via MAVLink
*/
/**
* Take-off altitude
*
* Even if first waypoint has altitude less then MIS_TAKEOFF_ALT above home position, system will climb to
* MIS_TAKEOFF_ALT on takeoff, then go to waypoint.
*
* @unit meters
* @group Mission
*/
PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f);
/**
* Enable onboard mission
*
* @min 0
* @max 1
* @group Mission
*/
PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 0);
src/modules/navigator/module.mk
+8 -2
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2013 PX4 Development Team. All rights reserved.
# Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -39,7 +39,13 @@ MODULE_COMMAND = navigator
SRCS = navigator_main.cpp \
navigator_params.c \
navigator_mission.cpp \
navigator_mode.cpp \
mission_block.cpp \
mission.cpp \
mission_params.c \
loiter.cpp \
rtl.cpp \
rtl_params.c \
mission_feasibility_checker.cpp \
geofence.cpp \
geofence_params.c
src/modules/navigator/navigator.h
+219
View File
@@ -0,0 +1,219 @@
/***************************************************************************
*
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file navigator.h
* Helper class to access missions
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef NAVIGATOR_H
#define NAVIGATOR_H
#include <systemlib/perf_counter.h>
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/parameter_update.h>
#include "navigator_mode.h"
#include "mission.h"
#include "loiter.h"
#include "rtl.h"
#include "geofence.h"
/**
* Number of navigation modes that need on_active/on_inactive calls
* Currently: mission, loiter, and rtl
*/
#define NAVIGATOR_MODE_ARRAY_SIZE 3
class Navigator : public control::SuperBlock
{
public:
/**
* Constructor
*/
Navigator();
/**
* Destructor, also kills the navigators task.
*/
~Navigator();
/**
* Start the navigator task.
*
* @return OK on success.
*/
int start();
/**
* Display the navigator status.
*/
void status();
/**
* Add point to geofence
*/
void add_fence_point(int argc, char *argv[]);
/**
* Load fence from file
*/
void load_fence_from_file(const char *filename);
/**
* Setters
*/
void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
/**
* Getters
*/
struct vehicle_status_s* get_vstatus() { return &_vstatus; }
struct vehicle_global_position_s* get_global_position() { return &_global_pos; }
struct home_position_s* get_home_position() { return &_home_pos; }
int get_onboard_mission_sub() { return _onboard_mission_sub; }
int get_offboard_mission_sub() { return _offboard_mission_sub; }
Geofence& get_geofence() { return _geofence; }
bool get_can_loiter_at_sp() { return _can_loiter_at_sp; }
float get_loiter_radius() { return _param_loiter_radius.get(); }
float get_acceptance_radius() { return _param_acceptance_radius.get(); }
int get_mavlink_fd() { return _mavlink_fd; }
private:
bool _task_should_exit; /**< if true, sensor task should exit */
int _navigator_task; /**< task handle for sensor task */
int _mavlink_fd; /**< the file descriptor to send messages over mavlink */
int _global_pos_sub; /**< global position subscription */
int _home_pos_sub; /**< home position subscription */
int _vstatus_sub; /**< vehicle status subscription */
int _capabilities_sub; /**< notification of vehicle capabilities updates */
int _control_mode_sub; /**< vehicle control mode subscription */
int _onboard_mission_sub; /**< onboard mission subscription */
int _offboard_mission_sub; /**< offboard mission subscription */
int _param_update_sub; /**< param update subscription */
orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */
vehicle_status_s _vstatus; /**< vehicle status */
vehicle_control_mode_s _control_mode; /**< vehicle control mode */
vehicle_global_position_s _global_pos; /**< global vehicle position */
home_position_s _home_pos; /**< home position for RTL */
mission_item_s _mission_item; /**< current mission item */
navigation_capabilities_s _nav_caps; /**< navigation capabilities */
position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */
bool _mission_item_valid; /**< flags if the current mission item is valid */
perf_counter_t _loop_perf; /**< loop performance counter */
Geofence _geofence; /**< class that handles the geofence */
bool _geofence_violation_warning_sent; /**< prevents spaming to mavlink */
bool _fence_valid; /**< flag if fence is valid */
bool _inside_fence; /**< vehicle is inside fence */
NavigatorMode *_navigation_mode; /**< abstract pointer to current navigation mode class */
Mission _mission; /**< class that handles the missions */
Loiter _loiter; /**< class that handles loiter */
RTL _rtl; /**< class that handles RTL */
NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */
bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */
bool _update_triplet; /**< flags if position SP triplet needs to be published */
control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */
control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */
/**
* Retrieve global position
*/
void global_position_update();
/**
* Retrieve home position
*/
void home_position_update();
/**
* Retreive navigation capabilities
*/
void navigation_capabilities_update();
/**
* Retrieve vehicle status
*/
void vehicle_status_update();
/**
* Retrieve vehicle control mode
*/
void vehicle_control_mode_update();
/**
* Update parameters
*/
void params_update();
/**
* Shim for calling task_main from task_create.
*/
static void task_main_trampoline(int argc, char *argv[]);
/**
* Main task.
*/
void task_main();
/**
* Translate mission item to a position setpoint.
*/
void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
/**
* Publish a new position setpoint triplet for position controllers
*/
void publish_position_setpoint_triplet();
};
#endif
src/modules/navigator/navigator_main.cpp
+138 -1304
View File
File diff suppressed because it is too large Load Diff
src/modules/navigator/navigator_mission.cpp
-319
View File
@@ -1,319 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file navigator_mission.cpp
* Helper class to access missions
*
* @author Julian Oes <joes@student.ethz.ch>
*/
#include <string.h>
#include <stdlib.h>
#include <dataman/dataman.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission_result.h>
#include "navigator_mission.h"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
Mission::Mission() :
_offboard_dataman_id(-1),
_current_offboard_mission_index(0),
_current_onboard_mission_index(0),
_offboard_mission_item_count(0),
_onboard_mission_item_count(0),
_onboard_mission_allowed(false),
_current_mission_type(MISSION_TYPE_NONE),
_mission_result_pub(-1)
{
memset(&_mission_result, 0, sizeof(struct mission_result_s));
}
Mission::~Mission()
{
}
void
Mission::set_offboard_dataman_id(int new_id)
{
_offboard_dataman_id = new_id;
}
void
Mission::set_current_offboard_mission_index(int new_index)
{
if (new_index != -1) {
warnx("specifically set to %d", new_index);
_current_offboard_mission_index = (unsigned)new_index;
} else {
/* if less WPs available, reset to first WP */
if (_current_offboard_mission_index >= _offboard_mission_item_count) {
_current_offboard_mission_index = 0;
}
}
report_current_offboard_mission_item();
}
void
Mission::set_current_onboard_mission_index(int new_index)
{
if (new_index != -1) {
_current_onboard_mission_index = (unsigned)new_index;
} else {
/* if less WPs available, reset to first WP */
if (_current_onboard_mission_index >= _onboard_mission_item_count) {
_current_onboard_mission_index = 0;
}
}
// TODO: implement this for onboard missions as well
// report_current_mission_item();
}
void
Mission::set_offboard_mission_count(unsigned new_count)
{
_offboard_mission_item_count = new_count;
}
void
Mission::set_onboard_mission_count(unsigned new_count)
{
_onboard_mission_item_count = new_count;
}
void
Mission::set_onboard_mission_allowed(bool allowed)
{
_onboard_mission_allowed = allowed;
}
bool
Mission::current_mission_available()
{
return (current_onboard_mission_available() || current_offboard_mission_available());
}
bool
Mission::next_mission_available()
{
return (next_onboard_mission_available() || next_offboard_mission_available());
}
int
Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, unsigned *index)
{
/* try onboard mission first */
if (current_onboard_mission_available()) {
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return ERROR;
}
_current_mission_type = MISSION_TYPE_ONBOARD;
*onboard = true;
*index = _current_onboard_mission_index;
/* otherwise fallback to offboard */
} else if (current_offboard_mission_available()) {
dm_item_t dm_current;
if (_offboard_dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(dm_current, _current_offboard_mission_index, new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
_current_mission_type = MISSION_TYPE_NONE;
return ERROR;
}
_current_mission_type = MISSION_TYPE_OFFBOARD;
*onboard = false;
*index = _current_offboard_mission_index;
} else {
/* happens when no more mission items can be added as a next item */
_current_mission_type = MISSION_TYPE_NONE;
return ERROR;
}
return OK;
}
int
Mission::get_next_mission_item(struct mission_item_s *new_mission_item)
{
/* try onboard mission first */
if (next_onboard_mission_available()) {
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return ERROR;
}
/* otherwise fallback to offboard */
} else if (next_offboard_mission_available()) {
dm_item_t dm_current;
if (_offboard_dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(dm_current, _current_offboard_mission_index + 1, new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return ERROR;
}
} else {
/* happens when no more mission items can be added as a next item */
return ERROR;
}
return OK;
}
bool
Mission::current_onboard_mission_available()
{
return _onboard_mission_item_count > _current_onboard_mission_index && _onboard_mission_allowed;
}
bool
Mission::current_offboard_mission_available()
{
return _offboard_mission_item_count > _current_offboard_mission_index;
}
bool
Mission::next_onboard_mission_available()
{
unsigned next = 0;
if (_current_mission_type != MISSION_TYPE_ONBOARD) {
next = 1;
}
return _onboard_mission_item_count > (_current_onboard_mission_index + next) && _onboard_mission_allowed;
}
bool
Mission::next_offboard_mission_available()
{
unsigned next = 0;
if (_current_mission_type != MISSION_TYPE_OFFBOARD) {
next = 1;
}
return _offboard_mission_item_count > (_current_offboard_mission_index + next);
}
void
Mission::move_to_next()
{
switch (_current_mission_type) {
case MISSION_TYPE_ONBOARD:
_current_onboard_mission_index++;
break;
case MISSION_TYPE_OFFBOARD:
_current_offboard_mission_index++;
break;
case MISSION_TYPE_NONE:
default:
break;
}
}
void
Mission::report_mission_item_reached()
{
if (_current_mission_type == MISSION_TYPE_OFFBOARD) {
_mission_result.mission_reached = true;
_mission_result.mission_index_reached = _current_offboard_mission_index;
}
}
void
Mission::report_current_offboard_mission_item()
{
_mission_result.index_current_mission = _current_offboard_mission_index;
}
void
Mission::publish_mission_result()
{
/* lazily publish the mission result only once available */
if (_mission_result_pub > 0) {
/* publish mission result */
orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
} else {
/* advertise and publish */
_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
}
/* reset reached bool */
_mission_result.mission_reached = false;
}
src/modules/navigator/navigator_mode.cpp
+70
View File
@@ -0,0 +1,70 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file navigator_mode.cpp
*
* Helper class for different modes in navigator
*
* @author Julian Oes <julian@oes.ch>
*/
#include "navigator_mode.h"
NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) :
SuperBlock(NULL, name),
_navigator(navigator),
_first_run(true)
{
/* load initial params */
updateParams();
/* set initial mission items */
on_inactive();
}
NavigatorMode::~NavigatorMode()
{
}
void
NavigatorMode::on_inactive()
{
_first_run = true;
}
bool
NavigatorMode::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
{
pos_sp_triplet->current.valid = false;
_first_run = false;
return false;
}
src/modules/navigator/navigator_mode.h
+86
View File
@@ -0,0 +1,86 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file navigator_mode.h
*
* Helper class for different modes in navigator
*
* @author Julian Oes <julian@oes.ch>
*/
#ifndef NAVIGATOR_MODE_H
#define NAVIGATOR_MODE_H
#include <drivers/drv_hrt.h>
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <dataman/dataman.h>
#include <uORB/topics/position_setpoint_triplet.h>
class Navigator;
class NavigatorMode : public control::SuperBlock
{
public:
/**
* Constructor
*/
NavigatorMode(Navigator *navigator, const char *name);
/**
* Destructor
*/
virtual ~NavigatorMode();
/**
* This function is called while the mode is inactive
*/
virtual void on_inactive();
/**
* This function is called while the mode is active
*
* @param position setpoint triplet to set
* @return true if position setpoint triplet has been changed
*/
virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
protected:
Navigator *_navigator;
bool _first_run;
};
#endif
src/modules/navigator/navigator_params.c
+12 -83
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -17,7 +17,7 @@
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
@@ -34,104 +34,33 @@
/**
* @file navigator_params.c
*
* Parameters defined by the navigator task.
* Parameters for navigator in general
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Julian Oes <julian@oes.ch>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/*
* Navigator parameters, accessible via MAVLink
*/
/**
* Minimum altitude (fixed wing only)
* Loiter radius (FW only)
*
* Minimum altitude above home for LOITER.
*
* @unit meters
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f);
/**
* Waypoint acceptance radius
*
* Default value of acceptance radius (if not specified in mission item).
* Default value of loiter radius for missions, loiter, RTL, etc. (fixedwing only).
*
* @unit meters
* @min 0.0
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f);
/**
* Loiter radius (fixed wing only)
*
* Default value of loiter radius (if not specified in mission item).
*
* @unit meters
* @min 0.0
* @group Navigation
* @group Mission
*/
PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
/**
* Enable onboard mission
* Acceptance Radius
*
* @group Navigation
*/
PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0);
/**
* Take-off altitude
*
* Even if first waypoint has altitude less then NAV_TAKEOFF_ALT above home position, system will climb to NAV_TAKEOFF_ALT on takeoff, then go to waypoint.
* Default acceptance radius, overridden by acceptance radius of waypoint if set.
*
* @unit meters
* @group Navigation
* @min 1.0
* @group Mission
*/
PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f);
/**
* Landing altitude
*
* Stay at this altitude above home position after RTL descending. Land (i.e. slowly descend) from this altitude if autolanding allowed.
*
* @unit meters
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f);
/**
* Return-To-Launch altitude
*
* Minimum altitude above home position for going home in RTL mode.
*
* @unit meters
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f);
/**
* Return-To-Launch delay
*
* Delay after descend before landing in RTL mode.
* If set to -1 the system will not land but loiter at NAV_LAND_ALT.
*
* @unit seconds
* @group Navigation
*/
PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f);
/**
* Enable parachute deployment
*
* @group Navigation
*/
PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0);
PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f);
src/modules/navigator/rtl.cpp
+320
View File
@@ -0,0 +1,320 @@
/****************************************************************************
*
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file navigator_rtl.cpp
* Helper class to access RTL
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <fcntl.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/home_position.h>
#include "navigator.h"
#include "rtl.h"
#define DELAY_SIGMA 0.01f
RTL::RTL(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
_rtl_state(RTL_STATE_NONE),
_param_return_alt(this, "RETURN_ALT"),
_param_descend_alt(this, "DESCEND_ALT"),
_param_land_delay(this, "LAND_DELAY")
{
/* load initial params */
updateParams();
/* initial reset */
on_inactive();
}
RTL::~RTL()
{
}
void
RTL::on_inactive()
{
_first_run = true;
/* reset RTL state only if setpoint moved */
if (!_navigator->get_can_loiter_at_sp()) {
_rtl_state = RTL_STATE_NONE;
}
}
bool
RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
{
bool updated = false;
if (_first_run) {
_first_run = false;
/* 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(pos_sp_triplet);
updated = true;
} else if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
advance_rtl();
set_rtl_item(pos_sp_triplet);
updated = true;
}
return updated;
}
void
RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
{
/* make sure we have the latest params */
updateParams();
set_previous_pos_setpoint(pos_sp_triplet);
_navigator->set_can_loiter_at_sp(false);
switch (_rtl_state) {
case RTL_STATE_CLIMB: {
float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = climb_alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home",
(int)(climb_alt - _navigator->get_home_position()->alt));
break;
}
case RTL_STATE_RETURN: {
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
// don't change altitude
if (pos_sp_triplet->previous.valid) {
/* if previous setpoint is valid then use it to calculate heading to home */
_mission_item.yaw = get_bearing_to_next_waypoint(
pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,
_mission_item.lat, _mission_item.lon);
} else {
/* else use current position */
_mission_item.yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
_mission_item.lat, _mission_item.lon);
}
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home",
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
break;
}
case RTL_STATE_DESCEND: {
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = false;
_mission_item.origin = ORIGIN_ONBOARD;
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home",
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
break;
}
case RTL_STATE_LOITER: {
bool autoland = _param_land_delay.get() > -DELAY_SIGMA;
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = autoland;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_can_loiter_at_sp(true);
if (autoland) {
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", _mission_item.time_inside);
} else {
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter");
}
break;
}
case RTL_STATE_LAND: {
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_home_position()->alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LAND;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home");
break;
}
case RTL_STATE_LANDED: {
_mission_item.lat = _navigator->get_home_position()->lat;
_mission_item.lon = _navigator->get_home_position()->lon;
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_home_position()->alt;
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_IDLE;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, landed");
break;
}
default:
break;
}
/* convert mission item to current position setpoint and make it valid */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
reset_mission_item_reached();
pos_sp_triplet->current.valid = true;
pos_sp_triplet->next.valid = false;
}
void
RTL::advance_rtl()
{
switch (_rtl_state) {
case RTL_STATE_CLIMB:
_rtl_state = RTL_STATE_RETURN;
break;
case RTL_STATE_RETURN:
_rtl_state = RTL_STATE_DESCEND;
break;
case RTL_STATE_DESCEND:
/* only go to land if autoland is enabled */
if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) {
_rtl_state = RTL_STATE_LOITER;
} else {
_rtl_state = RTL_STATE_LAND;
}
break;
case RTL_STATE_LOITER:
_rtl_state = RTL_STATE_LAND;
break;
case RTL_STATE_LAND:
_rtl_state = RTL_STATE_LANDED;
break;
default:
break;
}
}
src/modules/navigator/rtl.h
+110
View File
@@ -0,0 +1,110 @@
/***************************************************************************
*
* Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file navigator_rtl.h
* Helper class for RTL
*
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef NAVIGATOR_RTL_H
#define NAVIGATOR_RTL_H
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include "navigator_mode.h"
#include "mission_block.h"
class Navigator;
class RTL : public MissionBlock
{
public:
/**
* Constructor
*/
RTL(Navigator *navigator, const char *name);
/**
* Destructor
*/
~RTL();
/**
* This function is called while the mode is inactive
*/
void on_inactive();
/**
* This function is called while the mode is active
*
* @param position setpoint triplet that needs to be set
* @return true if updated
*/
bool on_active(position_setpoint_triplet_s *pos_sp_triplet);
private:
/**
* Set the RTL item
*/
void set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet);
/**
* Move to next RTL item
*/
void advance_rtl();
enum RTLState {
RTL_STATE_NONE = 0,
RTL_STATE_CLIMB,
RTL_STATE_RETURN,
RTL_STATE_DESCEND,
RTL_STATE_LOITER,
RTL_STATE_LAND,
RTL_STATE_LANDED,
} _rtl_state;
control::BlockParamFloat _param_return_alt;
control::BlockParamFloat _param_descend_alt;
control::BlockParamFloat _param_land_delay;
};
#endif
src/modules/navigator/rtl_params.c
+98
View File
@@ -0,0 +1,98 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file rtl_params.c
*
* Parameters for RTL
*
* @author Julian Oes <julian@oes.ch>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/*
* RTL parameters, accessible via MAVLink
*/
/**
* Loiter radius after RTL (FW only)
*
* Default value of loiter radius after RTL (fixedwing only).
*
* @unit meters
* @min 0.0
* @group RTL
*/
PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f);
/**
* RTL altitude
*
* Altitude to fly back in RTL in meters
*
* @unit meters
* @min 0
* @max 1
* @group RTL
*/
PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100);
/**
* RTL loiter altitude
*
* Stay at this altitude above home position after RTL descending.
* Land (i.e. slowly descend) from this altitude if autolanding allowed.
*
* @unit meters
* @min 0
* @max 100
* @group RTL
*/
PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20);
/**
* RTL delay
*
* Delay after descend before landing in RTL mode.
* If set to -1 the system will not land but loiter at NAV_LAND_ALT.
*
* @unit seconds
* @min -1
* @max
* @group RTL
*/
PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);
src/modules/position_estimator_inav/position_estimator_inav_main.c
+6 -6
View File
@@ -619,13 +619,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* hysteresis for GPS quality */
if (gps_valid) {
if (gps.eph_m > max_eph_epv || gps.epv_m > max_eph_epv || gps.fix_type < 3) {
if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) {
gps_valid = false;
mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
}
} else {
if (gps.eph_m < max_eph_epv * 0.7f && gps.epv_m < max_eph_epv * 0.7f && gps.fix_type >= 3) {
if (gps.eph < max_eph_epv * 0.7f && gps.epv < max_eph_epv * 0.7f && gps.fix_type >= 3) {
gps_valid = true;
reset_est = true;
mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
@@ -705,8 +705,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* save rotation matrix at this moment */
memcpy(R_gps, R_buf[est_i], sizeof(R_gps));
w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph_m);
w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv_m);
w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph);
w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv);
}
} else {
@@ -859,7 +859,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro);
if (use_gps_z) {
epv = fminf(epv, gps.epv_m);
epv = fminf(epv, gps.epv);
inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
}
@@ -894,7 +894,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
if (use_gps_xy) {
eph = fminf(eph, gps.eph_m);
eph = fminf(eph, gps.eph);
inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p);
inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p);
src/modules/sdlog2/sdlog2.c
+4 -4
View File
@@ -1117,7 +1117,7 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.msg_type = LOG_STAT_MSG;
log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state;
log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe_state;
log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe;
log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning;
log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed;
@@ -1138,8 +1138,8 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.msg_type = LOG_GPS_MSG;
log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec;
log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type;
log_msg.body.log_GPS.eph = buf_gps_pos.eph_m;
log_msg.body.log_GPS.epv = buf_gps_pos.epv_m;
log_msg.body.log_GPS.eph = buf_gps_pos.eph;
log_msg.body.log_GPS.epv = buf_gps_pos.epv;
log_msg.body.log_GPS.lat = buf_gps_pos.lat;
log_msg.body.log_GPS.lon = buf_gps_pos.lon;
log_msg.body.log_GPS.alt = buf_gps_pos.alt * 0.001f;
@@ -1352,7 +1352,7 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- GLOBAL POSITION SETPOINT --- */
if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) {
log_msg.msg_type = LOG_GPSP_MSG;
log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state;
log_msg.body.log_GPSP.nav_state = 0; /* TODO: Fix this */
log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d);
log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d);
log_msg.body.log_GPSP.alt = buf.triplet.current.alt;
src/modules/systemlib/state_table.h
+18 -11
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (C) 2013-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -33,8 +33,9 @@
/**
* @file state_table.h
*
*
* Finite-State-Machine helper class for state table
* @author: Julian Oes <julian@oes.ch>
*/
#ifndef __SYSTEMLIB_STATE_TABLE_H
@@ -48,22 +49,28 @@ public:
Action action;
unsigned nextState;
};
StateTable(Tran const *table, unsigned nStates, unsigned nSignals)
: myTable(table), myNsignals(nSignals), myNstates(nStates) {}
#define NO_ACTION &StateTable::doNothing
#define ACTION(_target) static_cast<StateTable::Action>(_target)
#define ACTION(_target) StateTable::Action(_target)
virtual ~StateTable() {}
void dispatch(unsigned const sig) {
register Tran const *t = myTable + myState*myNsignals + sig;
(this->*(t->action))();
void dispatch(unsigned const sig) {
/* get transition using state table */
Tran const *t = myTable + myState*myNsignals + sig;
/* accept new state */
myState = t->nextState;
/* */
(this->*(t->action))();
}
void doNothing() {
return;
}
void doNothing() {}
protected:
unsigned myState;
private:
@@ -72,4 +79,4 @@ private:
unsigned myNstates;
};
#endif
#endif
src/modules/uORB/topics/home_position.h
+4 -1
View File
@@ -59,10 +59,13 @@ struct home_position_s
{
uint64_t timestamp; /**< Timestamp (microseconds since system boot) */
//bool altitude_is_relative; // TODO what means home relative altitude? we need clear definition of reference altitude then
double lat; /**< Latitude in degrees */
double lon; /**< Longitude in degrees */
float alt; /**< Altitude in meters */
float x; /**< X coordinate in meters */
float y; /**< Y coordinate in meters */
float z; /**< Z coordinate in meters */
};
/**
src/modules/uORB/topics/mission.h
+9 -4
View File
@@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -37,6 +34,9 @@
/**
* @file mission.h
* Definition of a mission consisting of mission items.
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#ifndef TOPIC_MISSION_H_
@@ -50,6 +50,7 @@
/* compatible to mavlink MAV_CMD */
enum NAV_CMD {
NAV_CMD_IDLE=0,
NAV_CMD_WAYPOINT=16,
NAV_CMD_LOITER_UNLIMITED=17,
NAV_CMD_LOITER_TURN_COUNT=18,
@@ -58,7 +59,8 @@ enum NAV_CMD {
NAV_CMD_LAND=21,
NAV_CMD_TAKEOFF=22,
NAV_CMD_ROI=80,
NAV_CMD_PATHPLANNING=81
NAV_CMD_PATHPLANNING=81,
NAV_CMD_DO_JUMP=177
};
enum ORIGIN {
@@ -91,6 +93,9 @@ struct mission_item_s {
float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */
bool autocontinue; /**< true if next waypoint should follow after this one */
enum ORIGIN origin; /**< where the waypoint has been generated */
int do_jump_mission_index; /**< index where the do jump will go to */
unsigned do_jump_repeat_count; /**< how many times do jump needs to be done */
unsigned do_jump_current_count; /**< count how many times the jump has been done */
};
struct mission_s
src/modules/uORB/topics/mission_result.h
+1
View File
@@ -56,6 +56,7 @@ struct mission_result_s
bool mission_reached; /**< true if mission has been reached */
unsigned mission_index_reached; /**< index of the mission which has been reached */
unsigned index_current_mission; /**< index of the current mission */
bool mission_finished; /**< true if mission has been completed */
};
/**
src/modules/uORB/topics/position_setpoint_triplet.h
+10 -11
View File
@@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -37,6 +34,10 @@
/**
* @file mission_item_triplet.h
* Definition of the global WGS84 position setpoint uORB topic.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#ifndef TOPIC_MISSION_ITEM_TRIPLET_H_
@@ -45,7 +46,6 @@
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
#include <navigator/navigator_state.h>
/**
* @addtogroup topics
@@ -54,11 +54,12 @@
enum SETPOINT_TYPE
{
SETPOINT_TYPE_NORMAL = 0, /**< normal setpoint */
SETPOINT_TYPE_LOITER, /**< loiter setpoint */
SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */
SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, vehicle must descend until landing */
SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */
SETPOINT_TYPE_POSITION = 0, /**< position setpoint */
SETPOINT_TYPE_VELOCITY, /**< velocity setpoint */
SETPOINT_TYPE_LOITER, /**< loiter setpoint */
SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */
SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, descend until landing */
SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */
};
struct position_setpoint_s
@@ -84,8 +85,6 @@ struct position_setpoint_triplet_s
struct position_setpoint_s previous;
struct position_setpoint_s current;
struct position_setpoint_s next;
nav_state_t nav_state; /**< navigation state */
};
/**
src/modules/uORB/topics/telemetry_status.h
+1
View File
@@ -57,6 +57,7 @@ enum TELEMETRY_STATUS_RADIO_TYPE {
struct telemetry_status_s {
uint64_t timestamp;
uint64_t heartbeat_time; /**< Time of last received heartbeat from remote system */
enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */
uint8_t rssi; /**< local signal strength */
uint8_t remote_rssi; /**< remote signal strength */
src/modules/uORB/topics/vehicle_global_position.h
+6 -7
View File
@@ -36,7 +36,7 @@
* Definition of the global fused WGS84 position uORB topic.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
@@ -61,15 +61,14 @@
* e.g. control inputs of the vehicle in a Kalman-filter implementation.
*/
struct vehicle_global_position_s {
uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */
uint64_t time_gps_usec; /**< GPS timestamp in microseconds */
uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */
uint64_t time_gps_usec; /**< GPS timestamp in microseconds */
double lat; /**< Latitude in degrees */
double lon; /**< Longitude in degrees */
float alt; /**< Altitude AMSL in meters */
float vel_n; /**< Ground north velocity, m/s */
float vel_e; /**< Ground east velocity, m/s */
float vel_d; /**< Ground downside velocity, m/s */
float vel_n; /**< Ground north velocity, m/s */
float vel_e; /**< Ground east velocity, m/s */
float vel_d; /**< Ground downside velocity, m/s */
float yaw; /**< Yaw in radians -PI..+PI. */
float eph;
float epv;
src/modules/uORB/topics/vehicle_gps_position.h
+2 -2
View File
@@ -65,8 +65,8 @@ struct vehicle_gps_position_s {
float c_variance_rad; /**< course accuracy estimate rad */
uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */
float eph_m; /**< GPS HDOP horizontal dilution of position in m */
float epv_m; /**< GPS VDOP horizontal dilution of position in m */
float eph; /**< GPS HDOP horizontal dilution of position in m */
float epv; /**< GPS VDOP horizontal dilution of position in m */
unsigned noise_per_ms; /**< */
unsigned jamming_indicator; /**< */
src/modules/uORB/topics/vehicle_status.h
+36 -22
View File
@@ -1,10 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* Copyright (C) 2012 - 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -45,6 +41,11 @@
* All apps should write to subsystem_info:
*
* (any app) --> subsystem_info (published) --> (commander app state machine) --> vehicle_status --> (mavlink app)
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
*/
#ifndef VEHICLE_STATUS_H_
@@ -54,20 +55,22 @@
#include <stdbool.h>
#include "../uORB.h"
#include <navigator/navigator_state.h>
/**
* @addtogroup topics @{
*/
/* main state machine */
/**
* Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link.
*/
typedef enum {
MAIN_STATE_MANUAL = 0,
MAIN_STATE_ALTCTL,
MAIN_STATE_POSCTL,
MAIN_STATE_AUTO,
MAIN_STATE_AUTO_MISSION,
MAIN_STATE_AUTO_LOITER,
MAIN_STATE_AUTO_RTL,
MAIN_STATE_ACRO,
MAIN_STATE_MAX
MAIN_STATE_MAX,
} main_state_t;
// If you change the order, add or remove arming_state_t states make sure to update the arrays
@@ -80,7 +83,7 @@ typedef enum {
ARMING_STATE_STANDBY_ERROR,
ARMING_STATE_REBOOT,
ARMING_STATE_IN_AIR_RESTORE,
ARMING_STATE_MAX
ARMING_STATE_MAX,
} arming_state_t;
typedef enum {
@@ -88,13 +91,23 @@ typedef enum {
HIL_STATE_ON
} hil_state_t;
/**
* Navigation state, i.e. "what should vehicle do".
*/
typedef enum {
FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */
FAILSAFE_STATE_RTL, /**< Return To Launch */
FAILSAFE_STATE_LAND, /**< Land without position control */
FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */
FAILSAFE_STATE_MAX
} failsafe_state_t;
NAVIGATION_STATE_MANUAL = 0, /**< Manual mode */
NAVIGATION_STATE_ALTCTL, /**< Altitude control mode */
NAVIGATION_STATE_POSCTL, /**< Position control mode */
NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */
NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */
NAVIGATION_STATE_AUTO_RTL, /**< Auto return to launch mode */
NAVIGATION_STATE_AUTO_RTGS, /**< Auto return to groundstation on data link loss */
NAVIGATION_STATE_ACRO, /**< Acro mode */
NAVIGATION_STATE_LAND, /**< Land mode */
NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */
NAVIGATION_STATE_TERMINATION, /**< Termination mode */
NAVIGATION_STATE_MAX,
} navigation_state_t;
enum VEHICLE_MODE_FLAG {
VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
@@ -154,12 +167,11 @@ struct vehicle_status_s {
uint16_t counter; /**< incremented by the writing thread everytime new data is stored */
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
main_state_t main_state; /**< main state machine */
unsigned int set_nav_state; /**< set navigation state machine to specified value */
uint64_t set_nav_state_timestamp; /**< timestamp of latest change of set_nav_state */
main_state_t main_state; /**< main state machine */
navigation_state_t nav_state; /**< set navigation state machine to specified value */
arming_state_t arming_state; /**< current arming state */
hil_state_t hil_state; /**< current hil state */
failsafe_state_t failsafe_state; /**< current failsafe state */
hil_state_t hil_state; /**< current hil state */
bool failsafe; /**< true if system is in failsafe state */
int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */
int32_t system_id; /**< system id, inspired by MAVLink's system ID field */
@@ -184,6 +196,8 @@ struct vehicle_status_s {
bool rc_signal_lost; /**< true if RC reception lost */
bool rc_input_blocked; /**< set if RC input should be ignored */
bool data_link_lost; /**< datalink to GCS lost */
bool offboard_control_signal_found_once;
bool offboard_control_signal_lost;
bool offboard_control_signal_weak;