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

Merge branch 'navigator_rewrite' into dataman_state_nav_rewrite

Browse Source
This commit is contained in:
Anton Babushkin
2014-06-12 16:51:37 +02:00
parent 2951962c21 f4898b94c6
commit 842e7c3df9
57 changed files with 4571 additions and 2453 deletions
Download Patch File Download Diff File Expand all files Collapse all files
makefiles/config_px4fmu-v1_default.mk
+1 -5
View File
@@ -52,7 +52,6 @@ MODULES += systemcmds/pwm
MODULES += systemcmds/esc_calib
MODULES += systemcmds/reboot
MODULES += systemcmds/top
MODULES += systemcmds/tests
MODULES += systemcmds/config
MODULES += systemcmds/nshterm
MODULES += systemcmds/dumpfile
@@ -67,12 +66,11 @@ MODULES += modules/mavlink
MODULES += modules/gpio_led
#
# Estimation modules (EKF/ SO3 / other filters)
# Estimation modules (EKF / other filters)
#
MODULES += modules/attitude_estimator_ekf
MODULES += modules/ekf_att_pos_estimator
MODULES += modules/position_estimator_inav
#MODULES += examples/flow_position_estimator
#
# Vehicle Control
@@ -81,8 +79,6 @@ MODULES += modules/fw_pos_control_l1
MODULES += modules/fw_att_control
MODULES += modules/mc_att_control
MODULES += modules/mc_pos_control
#MODULES += examples/flow_position_control
#MODULES += examples/flow_speed_control
#
# Logging
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/lib/launchdetection/module.mk
+2
View File
@@ -38,3 +38,5 @@
SRCS = LaunchDetector.cpp \
CatapultLaunchMethod.cpp \
launchdetection_params.c
MAXOPTIMIZATION = -Os
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+2 -2
View File
@@ -393,7 +393,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;
@@ -402,7 +402,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;
src/modules/commander/commander.cpp
+253 -290
View File
@@ -1,11 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
* Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
* 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
@@ -38,8 +33,13 @@
/**
* @file commander.cpp
* Main system state machine implementation.
* Main fail-safe handling.
*
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <nuttx/config.h>
@@ -76,6 +76,7 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/mission_result.h>
#include <drivers/drv_led.h>
#include <drivers/drv_hrt.h>
@@ -87,6 +88,7 @@
#include <systemlib/err.h>
#include <systemlib/cpuload.h>
#include <systemlib/rc_check.h>
#include <systemlib/state_table.h>
#include "px4_custom_mode.h"
#include "commander_helper.h"
@@ -387,109 +389,80 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armed
bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
{
/* result of the command */
enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED;
bool ret = false;
/* only handle commands that are meant to be handled by this system and component */
if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components
return false;
}
/* only handle high-priority commands here */
/* result of the command */
enum VEHICLE_CMD_RESULT cmd_result = VEHICLE_CMD_RESULT_UNSUPPORTED;
/* request to set different system mode */
switch (cmd->command) {
case VEHICLE_CMD_DO_SET_MODE: {
uint8_t base_mode = (uint8_t) cmd->param1;
uint8_t custom_main_mode = (uint8_t) cmd->param2;
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
uint8_t base_mode = (uint8_t)cmd->param1;
uint8_t custom_main_mode = (uint8_t)cmd->param2;
transition_result_t arming_ret = TRANSITION_NOT_CHANGED;
transition_result_t main_ret = TRANSITION_NOT_CHANGED;
/* set HIL state */
hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd);
/* if HIL got enabled, reset battery status state */
if (hil_ret == OK && status->hil_state == HIL_STATE_ON) {
/* reset the arming mode to disarmed */
arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed);
if (arming_res != TRANSITION_DENIED) {
mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby");
} else {
mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state");
}
}
if (hil_ret == OK) {
ret = true;
}
transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd);
// Transition the arming state
arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
if (arming_res == TRANSITION_CHANGED) {
ret = true;
}
/* set main state */
transition_result_t main_res = TRANSITION_DENIED;
arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
/* use autopilot-specific mode */
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
/* MANUAL */
main_res = main_state_transition(status, MAIN_STATE_MANUAL);
main_ret = main_state_transition(status, MAIN_STATE_MANUAL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
/* ALTCTL */
main_res = main_state_transition(status, MAIN_STATE_ALTCTL);
main_ret = main_state_transition(status, MAIN_STATE_ALTCTL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
/* POSCTL */
main_res = main_state_transition(status, MAIN_STATE_POSCTL);
main_ret = main_state_transition(status, MAIN_STATE_POSCTL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
/* AUTO */
main_res = main_state_transition(status, MAIN_STATE_AUTO);
main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
/* ACRO */
main_res = main_state_transition(status, MAIN_STATE_ACRO);
main_ret = main_state_transition(status, MAIN_STATE_ACRO);
}
} else {
/* use base mode */
if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
/* AUTO */
main_res = main_state_transition(status, MAIN_STATE_AUTO);
main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION);
} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
/* POSCTL */
main_res = main_state_transition(status, MAIN_STATE_POSCTL);
main_ret = main_state_transition(status, MAIN_STATE_POSCTL);
} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
/* MANUAL */
main_res = main_state_transition(status, MAIN_STATE_MANUAL);
main_ret = main_state_transition(status, MAIN_STATE_MANUAL);
}
}
}
if (main_res == TRANSITION_CHANGED) {
ret = true;
}
if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
if (hil_ret != TRANSITION_DENIED && arming_ret != TRANSITION_DENIED && main_ret != TRANSITION_DENIED) {
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
break;
}
break;
case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
// Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
@@ -508,10 +481,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
if (arming_res == TRANSITION_DENIED) {
mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
} else {
result = VEHICLE_CMD_RESULT_ACCEPTED;
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
}
}
}
@@ -522,18 +495,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
unsigned int mav_goto = cmd->param1;
if (mav_goto == 0) { // MAV_GOTO_DO_HOLD
status->set_nav_state = NAV_STATE_LOITER;
status->set_nav_state_timestamp = hrt_absolute_time();
status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER;
mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd");
result = VEHICLE_CMD_RESULT_ACCEPTED;
ret = true;
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
} else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE
status->set_nav_state = NAV_STATE_MISSION;
status->set_nav_state_timestamp = hrt_absolute_time();
status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION;
mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd");
result = VEHICLE_CMD_RESULT_ACCEPTED;
ret = true;
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7);
@@ -541,6 +510,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
break;
#if 0
/* Flight termination */
case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
@@ -548,16 +518,16 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
//xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) {
transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
result = VEHICLE_CMD_RESULT_ACCEPTED;
ret = true;
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
/* reject parachute depoyment not armed */
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
}
break;
#endif
case VEHICLE_CMD_DO_SET_HOME: {
bool use_current = cmd->param1 > 0.5f;
@@ -571,10 +541,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
home->timestamp = hrt_absolute_time();
result = VEHICLE_CMD_RESULT_ACCEPTED;
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
} else {
@@ -585,10 +555,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
home->timestamp = hrt_absolute_time();
result = VEHICLE_CMD_RESULT_ACCEPTED;
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
}
if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
if (cmd_result == VEHICLE_CMD_RESULT_ACCEPTED) {
warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home->lat, home->lon, (double)home->alt);
mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt);
@@ -620,13 +590,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
break;
}
if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
if (cmd_result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
/* already warned about unsupported commands in "default" case */
answer_command(*cmd, result);
answer_command(*cmd, cmd_result);
}
/* send any requested ACKs */
if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
if (cmd->confirmation > 0 && cmd_result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
/* send acknowledge command */
// XXX TODO
}
@@ -655,8 +625,10 @@ int commander_thread_main(int argc, char *argv[])
main_states_str[0] = "MANUAL";
main_states_str[1] = "ALTCTL";
main_states_str[2] = "POSCTL";
main_states_str[3] = "AUTO";
main_states_str[4] = "ACRO";
main_states_str[3] = "AUTO_MISSION";
main_states_str[4] = "AUTO_LOITER";
main_states_str[5] = "AUTO_RTL";
main_states_str[6] = "ACRO";
char *arming_states_str[ARMING_STATE_MAX];
arming_states_str[0] = "INIT";
@@ -669,9 +641,10 @@ int commander_thread_main(int argc, char *argv[])
char *failsafe_states_str[FAILSAFE_STATE_MAX];
failsafe_states_str[0] = "NORMAL";
failsafe_states_str[1] = "RTL";
failsafe_states_str[2] = "LAND";
failsafe_states_str[3] = "TERMINATION";
failsafe_states_str[1] = "RTL_RC";
failsafe_states_str[2] = "RTL_DL";
failsafe_states_str[3] = "LAND";
failsafe_states_str[4] = "TERMINATION";
/* pthread for slow low prio thread */
pthread_t commander_low_prio_thread;
@@ -693,8 +666,7 @@ int commander_thread_main(int argc, char *argv[])
// We want to accept RC inputs as default
status.rc_input_blocked = false;
status.main_state = MAIN_STATE_MANUAL;
status.set_nav_state = NAV_STATE_NONE;
status.set_nav_state_timestamp = 0;
status.set_nav_state = NAVIGATION_STATE_MANUAL;
status.arming_state = ARMING_STATE_INIT;
status.hil_state = HIL_STATE_OFF;
status.failsafe_state = FAILSAFE_STATE_NORMAL;
@@ -784,6 +756,11 @@ int commander_thread_main(int argc, char *argv[])
safety.safety_switch_available = false;
safety.safety_off = false;
/* Subscribe to mission result topic */
int mission_result_sub = orb_subscribe(ORB_ID(mission_result));
struct mission_result_s mission_result;
memset(&mission_result, 0, sizeof(mission_result));
/* Subscribe to manual control data */
int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
struct manual_control_setpoint_s sp_man;
@@ -861,6 +838,13 @@ int commander_thread_main(int argc, char *argv[])
start_time = hrt_absolute_time();
transition_result_t arming_ret;
/* check which state machines for changes, clear "changed" flag */
bool arming_state_changed = false;
bool main_state_changed = false;
bool failsafe_state_changed = false;
while (!thread_should_exit) {
if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) {
@@ -868,6 +852,9 @@ int commander_thread_main(int argc, char *argv[])
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
arming_ret = TRANSITION_NOT_CHANGED;
/* update parameters */
orb_check(param_changed_sub, &updated);
@@ -947,6 +934,7 @@ int commander_thread_main(int argc, char *argv[])
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) {
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch");
arming_state_changed = true;
}
}
}
@@ -959,6 +947,14 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
}
/* update local position estimate */
orb_check(local_position_sub, &updated);
if (updated) {
/* position changed */
orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
}
/* update condition_global_position_valid */
/* hysteresis for EPH/EPV */
bool eph_epv_good;
@@ -992,6 +988,10 @@ int commander_thread_main(int argc, char *argv[])
home.lon = global_position.lon;
home.alt = global_position.alt;
home.x = local_position.x;
home.y = local_position.y;
home.z = local_position.z;
warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
@@ -1008,14 +1008,6 @@ int commander_thread_main(int argc, char *argv[])
tune_positive(true);
}
/* update local position estimate */
orb_check(local_position_sub, &updated);
if (updated) {
/* position changed */
orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
}
/* update condition_local_position_valid and condition_local_altitude_valid */
/* hysteresis for EPH */
bool local_eph_good;
@@ -1039,13 +1031,10 @@ int commander_thread_main(int argc, char *argv[])
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && local_eph_good, &(status.condition_local_position_valid), &status_changed);
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
static bool published_condition_landed_fw = false;
if (status.is_rotary_wing && status.condition_local_altitude_valid) {
if (status.condition_local_altitude_valid) {
if (status.condition_landed != local_position.landed) {
status.condition_landed = local_position.landed;
status_changed = true;
published_condition_landed_fw = false; //make sure condition_landed is published again if the system type changes
if (status.condition_landed) {
mavlink_log_critical(mavlink_fd, "#audio: LANDED");
@@ -1054,13 +1043,6 @@ int commander_thread_main(int argc, char *argv[])
mavlink_log_critical(mavlink_fd, "#audio: IN AIR");
}
}
} else {
if (!published_condition_landed_fw) {
status.condition_landed = false; // Fixedwing does not have a landing detector currently
published_condition_landed_fw = true;
status_changed = true;
}
}
/* update battery status */
@@ -1149,12 +1131,21 @@ int commander_thread_main(int argc, char *argv[])
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
if (armed.armed) {
arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
if (arming_ret == TRANSITION_CHANGED) {
warnx("changed 1");
arming_state_changed = true;
}
} else {
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
}
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
if (arming_ret == TRANSITION_CHANGED) {
warnx("changed 2");
arming_state_changed = true;
}
}
status_changed = true;
}
@@ -1162,11 +1153,15 @@ int commander_thread_main(int argc, char *argv[])
/* If in INIT state, try to proceed to STANDBY state */
if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
// XXX check for sensors
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
/* TODO: check for sensors */
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
} else {
// XXX: Add emergency stuff if sensors are lost
/* TODO: Add emergency stuff if sensors are lost */
}
@@ -1185,6 +1180,12 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
}
orb_check(mission_result_sub, &updated);
if (updated) {
orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
}
/* start RC input check */
if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
/* handle the case where RC signal was regained */
@@ -1197,16 +1198,14 @@ int commander_thread_main(int argc, char *argv[])
if (status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "#audio: RC signal regained");
status_changed = true;
status.failsafe_state = FAILSAFE_STATE_NORMAL;
failsafe_state_changed = true;
}
}
status.rc_signal_lost = false;
transition_result_t arming_res; // store all transitions results here
/* arm/disarm by RC */
arming_res = TRANSITION_NOT_CHANGED;
/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm
* do it only for rotary wings */
if (status.is_rotary_wing &&
@@ -1217,7 +1216,10 @@ int commander_thread_main(int argc, char *argv[])
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed);
arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
stick_off_counter = 0;
} else {
@@ -1239,7 +1241,10 @@ int commander_thread_main(int argc, char *argv[])
print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first.");
} else {
arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
}
}
stick_on_counter = 0;
@@ -1252,23 +1257,25 @@ int commander_thread_main(int argc, char *argv[])
stick_on_counter = 0;
}
if (arming_res == TRANSITION_CHANGED) {
if (arming_ret == TRANSITION_CHANGED) {
if (status.arming_state == ARMING_STATE_ARMED) {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
} else {
mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
}
arming_state_changed = true;
} else if (arming_res == TRANSITION_DENIED) {
} else if (arming_ret == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied");
}
if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
/* recover from failsafe */
(void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
}
// if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
// /* recover from failsafe */
// (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
// }
/* evaluate the main state machine according to mode switches */
transition_result_t main_res = set_main_state_rc(&status, &sp_man);
@@ -1276,101 +1283,46 @@ int commander_thread_main(int argc, char *argv[])
/* play tune on mode change only if armed, blink LED always */
if (main_res == TRANSITION_CHANGED) {
tune_positive(armed.armed);
main_state_changed = true;
} else if (main_res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
}
/* set navigation state */
/* RETURN switch, overrides MISSION switch */
if (sp_man.return_switch == SWITCH_POS_ON) {
/* switch to RTL if not already landed after RTL and home position set */
status.set_nav_state = NAV_STATE_RTL;
status.set_nav_state_timestamp = hrt_absolute_time();
} else {
/* LOITER switch */
if (sp_man.loiter_switch == SWITCH_POS_ON) {
/* stick is in LOITER position */
status.set_nav_state = NAV_STATE_LOITER;
status.set_nav_state_timestamp = hrt_absolute_time();
} else if (sp_man.loiter_switch != SWITCH_POS_NONE) {
/* stick is in MISSION position */
status.set_nav_state = NAV_STATE_MISSION;
status.set_nav_state_timestamp = hrt_absolute_time();
} else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) &&
pos_sp_triplet.nav_state == NAV_STATE_RTL) {
/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
status.set_nav_state = NAV_STATE_MISSION;
status.set_nav_state_timestamp = hrt_absolute_time();
}
}
} else {
if (!status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
status.rc_signal_lost = true;
status_changed = true;
}
if (armed.armed) {
if (status.main_state == MAIN_STATE_AUTO) {
/* check if AUTO mode still allowed */
transition_result_t auto_res = main_state_transition(&status, MAIN_STATE_AUTO);
if (!(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) {
if (auto_res == TRANSITION_NOT_CHANGED) {
last_auto_state_valid = hrt_absolute_time();
/* if we have a global position, we can switch to RTL, if not, we can try to land */
if (status.condition_global_position_valid) {
status.failsafe_state = FAILSAFE_STATE_RTL_RC;
} else {
status.failsafe_state = FAILSAFE_STATE_LAND;
}
/* still invalid state after the timeout interval, execute failsafe */
if ((hrt_elapsed_time(&last_auto_state_valid) > FAILSAFE_DEFAULT_TIMEOUT) && (auto_res == TRANSITION_DENIED)) {
/* AUTO mode denied, don't try RTL, switch to failsafe state LAND */
auto_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
if (auto_res == TRANSITION_DENIED) {
/* LAND not allowed, set TERMINATION state */
(void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
}
}
failsafe_state_changed = true;
} else {
/* failsafe for manual modes */
transition_result_t manual_res = TRANSITION_DENIED;
if (!status.condition_landed) {
/* vehicle is not landed, try to perform RTL */
manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
}
if (manual_res == TRANSITION_DENIED) {
/* RTL not allowed (no global position estimate) or not wanted, try LAND */
manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
if (manual_res == TRANSITION_DENIED) {
/* LAND not allowed, set TERMINATION state */
(void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
}
}
}
} else {
if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
/* reset failsafe when disarmed */
(void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
mavlink_log_info(mavlink_fd, "#audio: no RTL during Mission");
}
}
}
// TODO remove this hack
/* flight termination in manual mode if assist switch is on POSCTL position */
if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctl_switch == SWITCH_POS_ON) {
if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
tune_positive(armed.armed);
/* hack to detect if we finished a mission after we lost RC, so that we can trigger RTL now */
if (status.rc_signal_lost && status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION &&
mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RTL_RC) {
/* if we have a global position, we can switch to RTL, if not, we can try to land */
if (status.condition_global_position_valid) {
status.failsafe_state = FAILSAFE_STATE_RTL_RC;
mavlink_log_info(mavlink_fd, "#audio: RTL after Mission is finished");
} else {
/* this probably doesn't make sense since we are in mission and have global position */
status.failsafe_state = FAILSAFE_STATE_LAND;
}
failsafe_state_changed = true;
}
/* handle commands last, as the system needs to be updated to handle them */
@@ -1386,11 +1338,6 @@ int commander_thread_main(int argc, char *argv[])
}
}
/* check which state machines for changes, clear "changed" flag */
bool arming_state_changed = check_arming_state_changed();
bool main_state_changed = check_main_state_changed();
bool failsafe_state_changed = check_failsafe_state_changed();
hrt_abstime t1 = hrt_absolute_time();
/* print new state */
@@ -1407,6 +1354,10 @@ int commander_thread_main(int argc, char *argv[])
home.lon = global_position.lon;
home.alt = global_position.alt;
home.x = local_position.x;
home.y = local_position.y;
home.z = local_position.z;
warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
@@ -1421,18 +1372,24 @@ int commander_thread_main(int argc, char *argv[])
/* mark home position as set */
status.condition_home_position_valid = true;
}
arming_state_changed = false;
}
was_armed = armed.armed;
/* now set nav state according to failsafe and main state */
set_nav_state(&status);
if (main_state_changed) {
status_changed = true;
mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]);
main_state_changed = false;
}
if (failsafe_state_changed) {
status_changed = true;
mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]);
failsafe_state_changed = false;
}
/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
@@ -1621,6 +1578,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
switch (sp_man->mode_switch) {
case SWITCH_POS_NONE:
res = TRANSITION_NOT_CHANGED;
warnx("NONE");
break;
case SWITCH_POS_OFF: // MANUAL
@@ -1661,14 +1619,27 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
break;
case SWITCH_POS_ON: // AUTO
res = main_state_transition(status, MAIN_STATE_AUTO);
if (sp_man->return_switch == SWITCH_POS_ON) {
res = main_state_transition(status, MAIN_STATE_AUTO_RTL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
} else if (sp_man->loiter_switch == SWITCH_POS_ON) {
res = main_state_transition(status, MAIN_STATE_AUTO_LOITER);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
} else {
res = main_state_transition(status, MAIN_STATE_AUTO_MISSION);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
}
// else fallback to ALTCTL (POSCTL likely will not work too)
print_reject_mode(status, "AUTO");
res = main_state_transition(status, MAIN_STATE_ALTCTL);
if (res != TRANSITION_DENIED) {
@@ -1688,85 +1659,93 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
}
void
set_control_mode()
{
/* set vehicle_control_mode according to main state and failsafe state */
/* set vehicle_control_mode according to set_navigation_state */
control_mode.flag_armed = armed.armed;
/* TODO: check this */
control_mode.flag_external_manual_override_ok = !status.is_rotary_wing;
control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON;
control_mode.flag_control_termination_enabled = false;
/* set this flag when navigator should act */
bool navigator_enabled = false;
switch (status.failsafe_state) {
case FAILSAFE_STATE_NORMAL:
switch (status.main_state) {
case MAIN_STATE_MANUAL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = status.is_rotary_wing;
control_mode.flag_control_attitude_enabled = status.is_rotary_wing;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
break;
case MAIN_STATE_ALTCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
break;
case MAIN_STATE_POSCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = true;
control_mode.flag_control_velocity_enabled = true;
break;
case MAIN_STATE_AUTO:
navigator_enabled = true;
break;
case MAIN_STATE_ACRO:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = false;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
break;
default:
break;
}
switch (status.set_nav_state) {
case NAVIGATION_STATE_MANUAL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = status.is_rotary_wing;
control_mode.flag_control_attitude_enabled = status.is_rotary_wing;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_termination_enabled = false;
break;
case FAILSAFE_STATE_RTL:
navigator_enabled = true;
case NAVIGATION_STATE_ACRO:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = false;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_termination_enabled = false;
break;
case FAILSAFE_STATE_LAND:
navigator_enabled = true;
case NAVIGATION_STATE_ALTCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_termination_enabled = false;
break;
case FAILSAFE_STATE_TERMINATION:
case NAVIGATION_STATE_POSCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = true;
control_mode.flag_control_velocity_enabled = true;
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_AUTO_MISSION:
case NAVIGATION_STATE_AUTO_LOITER:
case NAVIGATION_STATE_AUTO_RTL:
case NAVIGATION_STATE_AUTO_RTL_RC:
case NAVIGATION_STATE_AUTO_RTL_DL:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = true;
control_mode.flag_control_velocity_enabled = true;
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_LAND:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
/* in failsafe LAND mode position may be not available */
control_mode.flag_control_position_enabled = status.condition_local_position_valid;
control_mode.flag_control_velocity_enabled = status.condition_local_position_valid;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_TERMINATION:
/* disable all controllers on termination */
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = false;
@@ -1782,21 +1761,6 @@ set_control_mode()
default:
break;
}
/* navigator has control, set control mode flags according to nav state*/
if (navigator_enabled) {
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
/* in failsafe LAND mode position may be not available */
control_mode.flag_control_position_enabled = status.condition_local_position_valid;
control_mode.flag_control_velocity_enabled = status.condition_local_position_valid;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
}
}
void
@@ -1940,7 +1904,6 @@ void *commander_low_prio_loop(void *arg)
/* try to go to INIT/PREFLIGHT arming state */
// XXX disable interrupts in arming_state_transition
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break;
src/modules/commander/state_machine_helper.cpp
+88 -336
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,98 @@ 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
*/
void set_nav_state(struct vehicle_status_s *status)
{
transition_result_t ret = TRANSITION_DENIED;
/* 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;
}
} else {
switch (new_failsafe_state) {
case FAILSAFE_STATE_NORMAL:
/* always allowed (except from TERMINATION state) */
ret = TRANSITION_CHANGED;
switch (status->failsafe_state) {
case FAILSAFE_STATE_NORMAL:
/* evaluate main state to decide in normal (non-failsafe) mode */
switch (status->main_state) {
case MAIN_STATE_MANUAL:
status->set_nav_state = NAVIGATION_STATE_MANUAL;
break;
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;
}
case MAIN_STATE_ALTCTL:
status->set_nav_state = NAVIGATION_STATE_ALTCTL;
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;
}
case MAIN_STATE_POSCTL:
status->set_nav_state = NAVIGATION_STATE_POSCTL;
break;
case FAILSAFE_STATE_TERMINATION:
/* always allowed */
ret = TRANSITION_CHANGED;
case MAIN_STATE_AUTO_MISSION:
status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION;
break;
case MAIN_STATE_AUTO_LOITER:
status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER;
break;
case MAIN_STATE_AUTO_RTL:
status->set_nav_state = NAVIGATION_STATE_AUTO_RTL;
break;
case MAIN_STATE_ACRO:
status->set_nav_state = NAVIGATION_STATE_ACRO;
break;
default:
break;
}
break;
if (ret == TRANSITION_CHANGED) {
if (status->failsafe_state != new_failsafe_state) {
status->failsafe_state = new_failsafe_state;
failsafe_state_changed = true;
case FAILSAFE_STATE_RTL_RC:
status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_RC;
break;
} else {
ret = TRANSITION_NOT_CHANGED;
}
}
case FAILSAFE_STATE_RTL_DL:
status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_DL;
break;
case FAILSAFE_STATE_LAND:
status->set_nav_state = NAVIGATION_STATE_LAND;
break;
case FAILSAFE_STATE_TERMINATION:
status->set_nav_state = NAVIGATION_STATE_TERMINATION;
break;
default:
break;
}
return ret;
}
// /*
// * 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 -12
View File
@@ -56,25 +56,17 @@ 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 = 0);
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 failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state);
bool check_navigation_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);
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);
void set_nav_state(struct vehicle_status_s *status);
#endif /* STATE_MACHINE_HELPER_H_ */
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+59 -11
View File
@@ -74,6 +74,7 @@
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/wind_estimate.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <geo/geo.h>
@@ -158,6 +159,7 @@ private:
orb_advert_t _global_pos_pub; /**< global position */
orb_advert_t _local_pos_pub; /**< position in local frame */
orb_advert_t _estimator_status_pub; /**< status of the estimator */
orb_advert_t _wind_pub; /**< wind estimate */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct gyro_report _gyro;
@@ -169,6 +171,7 @@ private:
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
struct vehicle_local_position_s _local_pos; /**< local vehicle position */
struct vehicle_gps_position_s _gps; /**< GPS position */
struct wind_estimate_s _wind; /**< Wind estimate */
struct gyro_scale _gyro_offsets;
struct accel_scale _accel_offsets;
@@ -246,6 +249,10 @@ private:
AttPosEKF *_ekf;
float _velocity_xy_filtered;
float _velocity_z_filtered;
float _airspeed_filtered;
/**
* Update our local parameter cache.
*/
@@ -312,6 +319,7 @@ FixedwingEstimator::FixedwingEstimator() :
_global_pos_pub(-1),
_local_pos_pub(-1),
_estimator_status_pub(-1),
_wind_pub(-1),
_att({}),
_gyro({}),
@@ -323,6 +331,7 @@ FixedwingEstimator::FixedwingEstimator() :
_global_pos({}),
_local_pos({}),
_gps({}),
_wind({}),
_gyro_offsets({}),
_accel_offsets({}),
@@ -352,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();
@@ -1028,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;
@@ -1068,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;
@@ -1282,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 */
@@ -1300,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) {
@@ -1321,20 +1349,38 @@ 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;
/* lazily publish the global position only once available */
if (_global_pos_pub > 0) {
/* publish the attitude setpoint */
/* publish the global position */
orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos);
} else {
/* advertise and publish */
_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos);
}
if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
_wind.timestamp = _global_pos.timestamp;
_wind.windspeed_north = _ekf->states[14];
_wind.windspeed_east = _ekf->states[15];
_wind.covariance_north = 0.0f; // XXX get form filter
_wind.covariance_east = 0.0f;
/* lazily publish the wind estimate only once available */
if (_wind_pub > 0) {
/* publish the wind estimate */
orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);
} else {
/* advertise and publish */
_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
}
}
}
}
@@ -1396,9 +1442,11 @@ FixedwingEstimator::print_status()
printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]);
printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]);
printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]);
printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]);
printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]);
printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]);
printf("states (accel offs) [14]: %8.4f\n", (double)_ekf->states[13]);
printf("states (wind) [15-16]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]);
printf("states (earth mag) [17-19]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]);
printf("states (body mag) [20-22]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], (double)_ekf->states[21]);
printf("states (terrain) [23]: %8.4f\n", (double)_ekf->states[22]);
printf("states: %s %s %s %s %s %s %s %s %s %s\n",
(_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT",
(_ekf->onGround) ? "ON_GROUND" : "AIRBORNE",
src/modules/ekf_att_pos_estimator/estimator.cpp
+2 -2
View File
@@ -2042,10 +2042,10 @@ float AttPosEKF::ConstrainFloat(float val, float min, float max)
float ret;
if (val > max) {
ret = max;
ekf_debug("> max: %8.4f, val: %8.4f", max, val);
ekf_debug("> max: %8.4f, val: %8.4f", (double)max, (double)val);
} else if (val < min) {
ret = min;
ekf_debug("< min: %8.4f, val: %8.4f", min, val);
ekf_debug("< min: %8.4f, val: %8.4f", (double)min, (double)val);
} else {
ret = val;
}
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+177 -85
View File
@@ -91,6 +91,7 @@
#include <external_lgpl/tecs/tecs.h>
#include <drivers/drv_range_finder.h>
#include "landingslope.h"
#include "mtecs/mTecs.h"
/**
@@ -153,8 +154,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;
@@ -201,6 +200,8 @@ private:
ECL_L1_Pos_Controller _l1_control;
TECS _tecs;
fwPosctrl::mTecs _mTecs;
bool _was_pos_control_mode;
struct {
float l1_period;
@@ -343,11 +344,11 @@ private:
/**
* Control position.
*/
bool control_position(const math::Vector<2> &global_pos, const math::Vector<2> &ground_speed,
bool control_position(const math::Vector<2> &global_pos, const math::Vector<3> &ground_speed,
const struct position_setpoint_triplet_s &_pos_sp_triplet);
float calculate_target_airspeed(float airspeed_demand);
void calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet);
void 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);
/**
* Shim for calling task_main from task_create.
@@ -368,6 +369,19 @@ private:
* Reset landing state
*/
void reset_landing_state();
/*
* Call TECS : a wrapper function to call one of the TECS implementations (mTECS is called only if enabled via parameter)
* XXX need to clean up/remove this function once mtecs fully replaces TECS
*/
void tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas,
float pitch_min_rad, float pitch_max_rad,
float throttle_min, float throttle_max, float throttle_cruise,
bool climbout_mode, float climbout_pitch_min_rad,
float altitude,
const math::Vector<3> &ground_speed,
tecs_mode mode = TECS_MODE_NORMAL);
};
namespace l1_control
@@ -406,7 +420,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),
@@ -431,6 +444,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
_global_pos(),
_pos_sp_triplet(),
_sensor_combined(),
_mTecs(),
_was_pos_control_mode(false),
_range_finder()
{
_nav_capabilities.turn_distance = 0.0f;
@@ -589,6 +604,9 @@ FixedwingPositionControl::parameters_update()
/* Update Launch Detector Parameters */
launchDetector.updateParams();
/* Update the mTecs */
_mTecs.updateParams();
return OK;
}
@@ -692,7 +710,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;
}
}
@@ -734,15 +751,15 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand)
}
void
FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet)
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));
yaw_vector.normalize();
float ground_speed_body = yaw_vector * ground_speed;
float ground_speed_body = yaw_vector * ground_speed_2d;
/* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/
float distance = 0.0f;
@@ -801,12 +818,13 @@ float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt,
}
bool
FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed,
FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<3> &ground_speed,
const struct position_setpoint_triplet_s &pos_sp_triplet)
{
bool setpoint = true;
calculate_gndspeed_undershoot(current_position, ground_speed, pos_sp_triplet);
math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)};
calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet);
float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
@@ -817,7 +835,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
math::Vector<3> accel_earth = _R_nb * accel_body;
_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
if (!_mTecs.getEnabled()) {
_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
}
float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
/* no throttle limit as default */
@@ -827,7 +848,17 @@ 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 */
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
}
}
_was_pos_control_mode = true;
/* get circle mode */
bool was_circle_mode = _l1_control.circle_mode();
@@ -859,31 +890,29 @@ 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);
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, math::radians(_parameters.pitch_limit_min),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) {
/* waypoint is a loiter waypoint */
_l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius,
pos_sp_triplet.current.loiter_direction, ground_speed);
pos_sp_triplet.current.loiter_direction, ground_speed_2d);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, math::radians(_parameters.pitch_limit_min),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
@@ -908,7 +937,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed);
_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed_2d);
/* limit roll motion to prevent wings from touching the ground first */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f));
@@ -918,7 +947,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else {
/* normal navigation */
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
}
_att_sp.roll_body = _l1_control.nav_roll();
@@ -942,7 +971,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
float L_altitude_rel = landingslope.getLandingSlopeRelativeAltitude(L_wp_distance);
float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f;
float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp);
@@ -977,11 +1006,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
land_stayonground = true;
}
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _pos_sp_triplet.current.alt + relative_alt, _pos_sp_triplet.current.alt + flare_curve_alt_rel, calculate_target_airspeed(airspeed_land),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, flare_pitch_angle_rad,
0.0f, throttle_max, throttle_land,
flare_pitch_angle_rad, math::radians(15.0f));
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + flare_curve_alt_rel,
calculate_target_airspeed(airspeed_land), eas2tas,
flare_pitch_angle_rad, math::radians(15.0f),
0.0f, throttle_max, throttle_land,
false, flare_pitch_angle_rad,
_pos_sp_triplet.current.alt + relative_alt, ground_speed,
land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND);
if (!land_noreturn_vertical) {
mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring");
@@ -994,11 +1025,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* intersect glide slope:
* minimize speed to approach speed
* if current position is higher or within 10m of slope follow the glide slope
* if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope
* if current position is higher than the slope follow the glide slope (sink to the
* glide slope)
* also if the system captures the slope it should stay
* on the slope (bool land_onslope)
* if current position is below the slope continue at previous wp altitude
* until the intersection with slope
* */
float altitude_desired_rel = relative_alt;
if (relative_alt > landing_slope_alt_rel_desired - 10.0f) {
if (relative_alt > landing_slope_alt_rel_desired || land_onslope) {
/* stay on slope */
altitude_desired_rel = landing_slope_alt_rel_desired;
if (!land_onslope) {
@@ -1007,14 +1042,20 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
} else {
/* continue horizontally */
altitude_desired_rel = math::max(relative_alt, L_altitude_rel);
altitude_desired_rel = _pos_sp_triplet.previous.valid ? L_altitude_rel : relative_alt;
}
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _pos_sp_triplet.current.alt + relative_alt, _pos_sp_triplet.current.alt + altitude_desired_rel, calculate_target_airspeed(airspeed_approach),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, math::radians(_parameters.pitch_limit_min),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + altitude_desired_rel,
calculate_target_airspeed(airspeed_approach), eas2tas,
math::radians(_parameters.pitch_limit_min),
math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min,
_parameters.throttle_max,
_parameters.throttle_cruise,
false,
math::radians(_parameters.pitch_limit_min),
_pos_sp_triplet.current.alt + relative_alt,
ground_speed);
}
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) {
@@ -1041,7 +1082,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
}
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
@@ -1052,22 +1093,36 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (altitude_error > 15.0f) {
/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min),
_airspeed.indicated_airspeed_m_s, eas2tas,
true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
calculate_target_airspeed(1.3f * _parameters.airspeed_min),
eas2tas,
math::radians(_parameters.pitch_limit_min),
math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max,
_parameters.throttle_cruise,
true,
math::max(math::radians(pos_sp_triplet.current.pitch_min),
math::radians(10.0f)),
_global_pos.alt,
ground_speed,
TECS_MODE_TAKEOFF);
/* limit roll motion to ensure enough lift */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
} else {
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, math::radians(_parameters.pitch_limit_min),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
calculate_target_airspeed(_parameters.airspeed_trim),
eas2tas,
math::radians(_parameters.pitch_limit_min),
math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min,
_parameters.throttle_max,
_parameters.throttle_cruise,
false,
math::radians(_parameters.pitch_limit_min),
_global_pos.alt,
ground_speed);
}
} else {
@@ -1101,19 +1156,21 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else if (0/* posctrl mode enabled */) {
_was_pos_control_mode = false;
/** POSCTRL FLIGHT **/
if (0/* switched from another mode to posctrl */) {
_altctrl_hold_heading = _att.yaw;
}
if (0/* switched from another mode to posctrl */) {
_altctrl_hold_heading = _att.yaw;
}
if (0/* posctrl on and manual control yaw non-zero */) {
_altctrl_hold_heading = _att.yaw + _manual.r;
}
if (0/* posctrl on and manual control yaw non-zero */) {
_altctrl_hold_heading = _att.yaw + _manual.r;
}
//XXX not used
//XXX not used
/* climb out control */
/* climb out control */
// bool climb_out = false;
//
// /* user wants to climb out */
@@ -1121,25 +1178,26 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
// climb_out = true;
// }
/* if in altctrl mode, set airspeed based on manual control */
/* if in altctrl mode, set airspeed based on manual control */
// XXX check if ground speed undershoot should be applied here
float altctrl_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.z;
// XXX check if ground speed undershoot should be applied here
float altctrl_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.z;
_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed);
_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.x * 2.0f,
altctrl_airspeed,
_airspeed.indicated_airspeed_m_s, eas2tas,
false, _parameters.pitch_limit_min,
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
_parameters.pitch_limit_min, _parameters.pitch_limit_max);
tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
} else if (0/* altctrl mode enabled */) {
_was_pos_control_mode = false;
/** ALTCTRL FLIGHT **/
if (0/* switched from another mode to altctrl */) {
@@ -1172,18 +1230,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
climb_out = true;
}
_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed);
_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d);
_att_sp.roll_body = _manual.y;
_att_sp.yaw_body = _manual.r;
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.x * 2.0f,
altctrl_airspeed,
_airspeed.indicated_airspeed_m_s, eas2tas,
climb_out, _parameters.pitch_limit_min,
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
_parameters.pitch_limit_min, _parameters.pitch_limit_max);
tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
climb_out, math::radians(_parameters.pitch_limit_min),
_global_pos.alt, ground_speed);
} else {
_was_pos_control_mode = false;
/** MANUAL FLIGHT **/
/* no flight mode applies, do not publish an attitude setpoint */
@@ -1200,9 +1259,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
}
else {
_att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max);
_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :_tecs.get_throttle_demand(), throttle_max);
}
_att_sp.pitch_body = _tecs.get_pitch_demand();
_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
if (_control_mode.flag_control_position_enabled) {
last_manual = false;
@@ -1316,7 +1375,7 @@ FixedwingPositionControl::task_main()
range_finder_poll();
// vehicle_baro_poll();
math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e);
math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e, _global_pos.vel_d);
math::Vector<2> current_position((float)_global_pos.lat, (float)_global_pos.lon);
/*
@@ -1378,6 +1437,39 @@ void FixedwingPositionControl::reset_landing_state()
land_onslope = false;
}
void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas,
float pitch_min_rad, float pitch_max_rad,
float throttle_min, float throttle_max, float throttle_cruise,
bool climbout_mode, float climbout_pitch_min_rad,
float altitude,
const math::Vector<3> &ground_speed,
tecs_mode mode)
{
if (_mTecs.getEnabled()) {
/* Using mtecs library: prepare arguments for mtecs call */
float flightPathAngle = 0.0f;
float ground_speed_length = ground_speed.length();
if (ground_speed_length > FLT_EPSILON) {
flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
}
fwPosctrl::LimitOverride limitOverride;
if (climbout_mode) {
limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
} else {
limitOverride.disablePitchMinOverride();
}
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
limitOverride);
} else {
/* Using tecs library */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
_airspeed.indicated_airspeed_m_s, eas2tas,
climbout_mode, climbout_pitch_min_rad,
throttle_min, throttle_max, throttle_cruise,
pitch_min_rad, pitch_max_rad);
}
}
int
FixedwingPositionControl::start()
{
src/modules/fw_pos_control_l1/module.mk
+4 -1
View File
@@ -39,4 +39,7 @@ MODULE_COMMAND = fw_pos_control_l1
SRCS = fw_pos_control_l1_main.cpp \
fw_pos_control_l1_params.c \
landingslope.cpp
landingslope.cpp \
mtecs/mTecs.cpp \
mtecs/limitoverride.cpp \
mtecs/mTecs_params.c
src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp
+71
View File
@@ -0,0 +1,71 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@gmail.com>
*
* 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 limitoverride.cpp
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include "limitoverride.h"
namespace fwPosctrl {
bool LimitOverride::applyOverride(BlockOutputLimiter &outputLimiterThrottle,
BlockOutputLimiter &outputLimiterPitch)
{
bool ret = false;
if (overrideThrottleMinEnabled) {
outputLimiterThrottle.setMin(overrideThrottleMin);
ret = true;
}
if (overrideThrottleMaxEnabled) {
outputLimiterThrottle.setMax(overrideThrottleMax);
ret = true;
}
if (overridePitchMinEnabled) {
outputLimiterPitch.setMin(overridePitchMin);
ret = true;
}
if (overridePitchMaxEnabled) {
outputLimiterPitch.setMax(overridePitchMax);
ret = true;
}
return ret;
}
} /* namespace fwPosctrl */
src/modules/fw_pos_control_l1/mtecs/limitoverride.h
+107
View File
@@ -0,0 +1,107 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@gmail.com>
*
* 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 limitoverride.h
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#ifndef LIMITOVERRIDE_H_
#define LIMITOVERRIDE_H_
#include "mTecs_blocks.h"
namespace fwPosctrl
{
/* A small class which provides helper functions to override control output limits which are usually set by
* parameters in special cases
*/
class LimitOverride
{
public:
LimitOverride() :
overrideThrottleMinEnabled(false),
overrideThrottleMaxEnabled(false),
overridePitchMinEnabled(false),
overridePitchMaxEnabled(false)
{};
~LimitOverride() {};
/*
* Override the limits of the outputlimiter instances given by the arguments with the limits saved in
* this class (if enabled)
* @return true if the limit was applied
*/
bool applyOverride(BlockOutputLimiter &outputLimiterThrottle,
BlockOutputLimiter &outputLimiterPitch);
/* Functions to enable or disable the override */
void enableThrottleMinOverride(float value) { enable(&overrideThrottleMinEnabled,
&overrideThrottleMin, value); }
void disableThrottleMinOverride() { disable(&overrideThrottleMinEnabled); }
void enableThrottleMaxOverride(float value) { enable(&overrideThrottleMaxEnabled,
&overrideThrottleMax, value); }
void disableThrottleMaxOverride() { disable(&overrideThrottleMaxEnabled); }
void enablePitchMinOverride(float value) { enable(&overridePitchMinEnabled,
&overridePitchMin, value); }
void disablePitchMinOverride() { disable(&overridePitchMinEnabled); }
void enablePitchMaxOverride(float value) { enable(&overridePitchMaxEnabled,
&overridePitchMax, value); }
void disablePitchMaxOverride() { disable(&overridePitchMaxEnabled); }
protected:
bool overrideThrottleMinEnabled;
float overrideThrottleMin;
bool overrideThrottleMaxEnabled;
float overrideThrottleMax;
bool overridePitchMinEnabled;
float overridePitchMin; //in degrees (replaces param values)
bool overridePitchMaxEnabled;
float overridePitchMax; //in degrees (replaces param values)
/* Enable a specific limit override */
void enable(bool *flag, float *limit, float value) { *flag = true; *limit = value; };
/* Disable a specific limit override */
void disable(bool *flag) { *flag = false; };
};
} /* namespace fwPosctrl */
#endif /* LIMITOVERRIDE_H_ */
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+313
View File
@@ -0,0 +1,313 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@gmail.com>
*
* 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 mTecs.cpp
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include "mTecs.h"
#include <lib/geo/geo.h>
#include <stdio.h>
namespace fwPosctrl {
mTecs::mTecs() :
SuperBlock(NULL, "MT"),
/* Parameters */
_mTecsEnabled(this, "ENABLED"),
_airspeedMin(this, "FW_AIRSPD_MIN", false),
/* Publications */
_status(&getPublications(), ORB_ID(tecs_status)),
/* control blocks */
_controlTotalEnergy(this, "THR"),
_controlEnergyDistribution(this, "PIT", true),
_controlAltitude(this, "FPA", true),
_controlAirSpeed(this, "ACC"),
_airspeedLowpass(this, "A_LP"),
_airspeedDerivative(this, "AD"),
_throttleSp(0.0f),
_pitchSp(0.0f),
_BlockOutputLimiterTakeoffThrottle(this, "TKF_THR"),
_BlockOutputLimiterTakeoffPitch(this, "TKF_PIT", true),
_BlockOutputLimiterUnderspeedThrottle(this, "USP_THR"),
_BlockOutputLimiterUnderspeedPitch(this, "USP_PIT", true),
_BlockOutputLimiterLandThrottle(this, "LND_THR"),
_BlockOutputLimiterLandPitch(this, "LND_PIT", true),
timestampLastIteration(hrt_absolute_time()),
_firstIterationAfterReset(true),
_dtCalculated(false),
_counter(0),
_debug(false)
{
}
mTecs::~mTecs()
{
}
int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed,
float airspeedSp, tecs_mode mode, LimitOverride limitOverride)
{
/* check if all input arguments are numbers and abort if not so */
if (!isfinite(flightPathAngle) || !isfinite(altitude) ||
!isfinite(altitudeSp) || !isfinite(airspeed) || !isfinite(airspeedSp) || !isfinite(mode)) {
return -1;
}
/* time measurement */
updateTimeMeasurement();
/* calculate flight path angle setpoint from altitude setpoint */
float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude);
/* Debug output */
if (_counter % 10 == 0) {
debug("***");
debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp);
}
/* Write part of the status message */
_status.altitudeSp = altitudeSp;
_status.altitude = altitude;
/* use flightpath angle setpoint for total energy control */
return updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed,
airspeedSp, mode, limitOverride);
}
int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed,
float airspeedSp, tecs_mode mode, LimitOverride limitOverride)
{
/* check if all input arguments are numbers and abort if not so */
if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) ||
!isfinite(airspeed) || !isfinite(airspeedSp) || !isfinite(mode)) {
return -1;
}
/* time measurement */
updateTimeMeasurement();
/* Filter arispeed */
float airspeedFiltered = _airspeedLowpass.update(airspeed);
/* calculate longitudinal acceleration setpoint from airspeed setpoint*/
float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeedFiltered);
/* Debug output */
if (_counter % 10 == 0) {
debug("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f airspeedFiltered %.4f,"
"accelerationLongitudinalSp%.4f",
(double)airspeedSp, (double)airspeed,
(double)airspeedFiltered, (double)accelerationLongitudinalSp);
}
/* Write part of the status message */
_status.flightPathAngleSp = flightPathAngleSp;
_status.flightPathAngle = flightPathAngle;
_status.airspeedSp = airspeedSp;
_status.airspeed = airspeed;
_status.airspeedFiltered = airspeedFiltered;
/* use longitudinal acceleration setpoint for total energy control */
return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeedFiltered,
accelerationLongitudinalSp, mode, limitOverride);
}
int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered,
float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride)
{
/* check if all input arguments are numbers and abort if not so */
if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) ||
!isfinite(airspeedFiltered) || !isfinite(accelerationLongitudinalSp) || !isfinite(mode)) {
return -1;
}
/* time measurement */
updateTimeMeasurement();
/* update parameters first */
updateParams();
/* calculate values (energies) */
float flightPathAngleError = flightPathAngleSp - flightPathAngle;
float airspeedDerivative = 0.0f;
if(_airspeedDerivative.getDt() > 0.0f) {
airspeedDerivative = _airspeedDerivative.update(airspeedFiltered);
}
float airspeedDerivativeNorm = airspeedDerivative / CONSTANTS_ONE_G;
float airspeedDerivativeSp = accelerationLongitudinalSp;
float airspeedDerivativeNormSp = airspeedDerivativeSp / CONSTANTS_ONE_G;
float airspeedDerivativeNormError = airspeedDerivativeNormSp - airspeedDerivativeNorm;
float totalEnergyRate = flightPathAngle + airspeedDerivativeNorm;
float totalEnergyRateError = flightPathAngleError + airspeedDerivativeNormError;
float totalEnergyRateSp = flightPathAngleSp + airspeedDerivativeNormSp;
float totalEnergyRateError2 = totalEnergyRateSp - totalEnergyRate;
float energyDistributionRate = flightPathAngle - airspeedDerivativeNorm;
float energyDistributionRateError = flightPathAngleError - airspeedDerivativeNormError;
float energyDistributionRateSp = flightPathAngleSp - airspeedDerivativeNormSp;
float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate;
/* Debug output */
if (_counter % 10 == 0) {
debug("totalEnergyRateSp %.2f, totalEnergyRate %.2f, totalEnergyRateError %.2f totalEnergyRateError2 %.2f airspeedDerivativeNorm %.4f",
(double)totalEnergyRateSp, (double)totalEnergyRate, (double)totalEnergyRateError, (double)totalEnergyRateError2, (double)airspeedDerivativeNorm);
debug("energyDistributionRateSp %.2f, energyDistributionRate %.2f, energyDistributionRateError %.2f energyDistributionRateError2 %.2f",
(double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2);
}
/* Check airspeed: if below safe value switch to underspeed mode (if not in land or takeoff mode) */
if (mode != TECS_MODE_LAND && mode != TECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) {
mode = TECS_MODE_UNDERSPEED;
}
/* Set special ouput limiters if we are not in TECS_MODE_NORMAL */
BlockOutputLimiter *outputLimiterThrottle = NULL; // NULL --> use standard inflight limits
BlockOutputLimiter *outputLimiterPitch = NULL; // NULL --> use standard inflight limits
if (mode == TECS_MODE_TAKEOFF) {
outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector
outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch;
} else if (mode == TECS_MODE_LAND) {
// only limit pitch but do not limit throttle
outputLimiterPitch = &_BlockOutputLimiterLandPitch;
} else if (mode == TECS_MODE_LAND_THROTTLELIM) {
outputLimiterThrottle = &_BlockOutputLimiterLandThrottle;
outputLimiterPitch = &_BlockOutputLimiterLandPitch;
} else if (mode == TECS_MODE_UNDERSPEED) {
outputLimiterThrottle = &_BlockOutputLimiterUnderspeedThrottle;
outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch;
}
/* Apply overrride given by the limitOverride argument (this is used for limits which are not given by
* parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector
* is running) */
bool limitApplied = limitOverride.applyOverride(outputLimiterThrottle == NULL ?
_controlTotalEnergy.getOutputLimiter() :
*outputLimiterThrottle,
outputLimiterPitch == NULL ?
_controlEnergyDistribution.getOutputLimiter() :
*outputLimiterPitch);
/* Write part of the status message */
_status.airspeedDerivativeSp = airspeedDerivativeSp;
_status.airspeedDerivative = airspeedDerivative;
_status.totalEnergyRateSp = totalEnergyRateSp;
_status.totalEnergyRate = totalEnergyRate;
_status.energyDistributionRateSp = energyDistributionRateSp;
_status.energyDistributionRate = energyDistributionRate;
_status.mode = mode;
/** update control blocks **/
/* update total energy rate control block */
_throttleSp = _controlTotalEnergy.update(totalEnergyRateSp, totalEnergyRateError, outputLimiterThrottle);
/* update energy distribution rate control block */
_pitchSp = _controlEnergyDistribution.update(energyDistributionRateSp, energyDistributionRateError, outputLimiterPitch);
if (_counter % 10 == 0) {
debug("_throttleSp %.1f, _pitchSp %.1f, flightPathAngleSp %.1f, flightPathAngle %.1f accelerationLongitudinalSp %.1f, airspeedDerivative %.1f",
(double)_throttleSp, (double)_pitchSp,
(double)flightPathAngleSp, (double)flightPathAngle,
(double)accelerationLongitudinalSp, (double)airspeedDerivative);
}
/* publish status messge */
_status.update();
/* clean up */
_firstIterationAfterReset = false;
_dtCalculated = false;
_counter++;
return 0;
}
void mTecs::resetIntegrators()
{
_controlTotalEnergy.getIntegral().setY(0.0f);
_controlEnergyDistribution.getIntegral().setY(0.0f);
timestampLastIteration = hrt_absolute_time();
_firstIterationAfterReset = true;
}
void mTecs::resetDerivatives(float airspeed)
{
_airspeedDerivative.setU(airspeed);
}
void mTecs::updateTimeMeasurement()
{
if (!_dtCalculated) {
float deltaTSeconds = 0.0f;
if (!_firstIterationAfterReset) {
hrt_abstime timestampNow = hrt_absolute_time();
deltaTSeconds = (float)(timestampNow - timestampLastIteration) * 1e-6f;
timestampLastIteration = timestampNow;
}
setDt(deltaTSeconds);
_dtCalculated = true;
}
}
void mTecs::debug_print(const char *fmt, va_list args)
{
fprintf(stderr, "%s: ", "[mtecs]");
vfprintf(stderr, fmt, args);
fprintf(stderr, "\n");
}
void mTecs::debug(const char *fmt, ...) {
if (!_debug) {
return;
}
va_list args;
va_start(args, fmt);
debug_print(fmt, args);
}
} /* namespace fwPosctrl */
src/modules/fw_pos_control_l1/mtecs/mTecs.h
+149
View File
@@ -0,0 +1,149 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@gmail.com>
*
* 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 mTecs.h
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#ifndef MTECS_H_
#define MTECS_H_
#include "mTecs_blocks.h"
#include "limitoverride.h"
#include <controllib/block/BlockParam.hpp>
#include <drivers/drv_hrt.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/tecs_status.h>
namespace fwPosctrl
{
/* Main class of the mTecs */
class mTecs : public control::SuperBlock
{
public:
mTecs();
virtual ~mTecs();
/*
* Control in altitude setpoint and speed mode
*/
int updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed,
float airspeedSp, tecs_mode mode, LimitOverride limitOverride);
/*
* Control in flightPathAngle setpoint (flollow a slope etc.) and speed mode
*/
int updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed,
float airspeedSp, tecs_mode mode, LimitOverride limitOverride);
/*
* Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case)
*/
int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered,
float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride);
/*
* Reset all integrators
*/
void resetIntegrators();
/*
* Reset all derivative calculations
*/
void resetDerivatives(float airspeed);
/* Accessors */
bool getEnabled() { return _mTecsEnabled.get() > 0; }
float getThrottleSetpoint() { return _throttleSp; }
float getPitchSetpoint() { return _pitchSp; }
float airspeedLowpassUpdate(float input) { return _airspeedLowpass.update(input); }
protected:
/* parameters */
control::BlockParamInt _mTecsEnabled; /**< 1 if mTecs is enabled */
control::BlockParamFloat _airspeedMin; /**< minimal airspeed */
/* Publications */
uORB::Publication<tecs_status_s> _status; /**< publish internal values for logging */
/* control blocks */
BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output is throttle */
BlockFFPILimitedCustom _controlEnergyDistribution; /**< FFPI controller for energy distribution control: output is pitch */
BlockPDLimited _controlAltitude; /**< PD controller for altitude: output is the flight path angle setpoint */
BlockPDLimited _controlAirSpeed; /**< PD controller for airspeed: output is acceleration setpoint */
/* Other calculation Blocks */
control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */
control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */
/* Output setpoints */
float _throttleSp; /**< Throttle Setpoint from 0 to 1 */
float _pitchSp; /**< Pitch Setpoint from -pi to pi */
/* Output Limits in special modes */
BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle; /**< Throttle Limits during takeoff */
BlockOutputLimiter _BlockOutputLimiterTakeoffPitch; /**< Pitch Limit during takeoff */
BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle; /**< Throttle Limits when underspeed is detected */
BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch; /**< Pitch Limit when underspeed is detected */
BlockOutputLimiter _BlockOutputLimiterLandThrottle; /**< Throttle Limits during landing (only in last phase)*/
BlockOutputLimiter _BlockOutputLimiterLandPitch; /**< Pitch Limit during landing */
/* Time measurements */
hrt_abstime timestampLastIteration; /**< Saves the result of hrt_absolute_time() of the last iteration */
bool _firstIterationAfterReset; /**< True during the first iteration after a reset */
bool _dtCalculated; /**< True if dt has been calculated in this iteration */
int _counter;
bool _debug; ///< Set true to enable debug output
static void debug_print(const char *fmt, va_list args);
void debug(const char *fmt, ...);
/*
* Measure and update the time step dt if this was not already done in the current iteration
*/
void updateTimeMeasurement();
};
} /* namespace fwPosctrl */
#endif /* MTECS_H_ */
src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
+220
View File
@@ -0,0 +1,220 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@gmail.com>
*
* 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 mTecs_blocks.h
*
* Custom blocks for the mTecs
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#pragma once
#include <controllib/blocks.hpp>
#include <systemlib/err.h>
namespace fwPosctrl
{
using namespace control;
/* An block which can be used to limit the output */
class BlockOutputLimiter: public SuperBlock
{
public:
// methods
BlockOutputLimiter(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
SuperBlock(parent, name),
_isAngularLimit(isAngularLimit),
_min(this, "MIN"),
_max(this, "MAX")
{};
virtual ~BlockOutputLimiter() {};
/*
* Imposes the limits given by _min and _max on value
*
* @param value is changed to be on the interval _min to _max
* @param difference if the value is changed this corresponds to the change of value * (-1)
* otherwise unchanged
* @return: true if the limit is applied, false otherwise
*/
bool limit(float& value, float& difference) {
float minimum = isAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin();
float maximum = isAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax();
if (value < minimum) {
difference = value - minimum;
value = minimum;
return true;
} else if (value > maximum) {
difference = value - maximum;
value = maximum;
return true;
}
return false;
}
//accessor:
bool isAngularLimit() {return _isAngularLimit ;}
float getMin() { return _min.get(); }
float getMax() { return _max.get(); }
void setMin(float value) { _min.set(value); }
void setMax(float value) { _max.set(value); }
protected:
//attributes
bool _isAngularLimit;
control::BlockParamFloat _min;
control::BlockParamFloat _max;
};
/* A combination of feed forward, P and I gain using the output limiter*/
class BlockFFPILimited: public SuperBlock
{
public:
// methods
BlockFFPILimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
SuperBlock(parent, name),
_outputLimiter(this, "", isAngularLimit),
_integral(this, "I"),
_kFF(this, "FF"),
_kP(this, "P"),
_kI(this, "I"),
_offset(this, "OFF")
{};
virtual ~BlockFFPILimited() {};
float update(float inputValue, float inputError) { return calcLimitedOutput(inputValue, inputError, _outputLimiter); }
// accessors
BlockIntegral &getIntegral() { return _integral; }
float getKFF() { return _kFF.get(); }
float getKP() { return _kP.get(); }
float getKI() { return _kI.get(); }
float getOffset() { return _offset.get(); }
BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; };
protected:
BlockOutputLimiter _outputLimiter;
float calcUnlimitedOutput(float inputValue, float inputError) {return getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError);}
float calcLimitedOutput(float inputValue, float inputError, BlockOutputLimiter &outputLimiter) {
float difference = 0.0f;
float integralYPrevious = _integral.getY();
float output = calcUnlimitedOutput(inputValue, inputError);
if(outputLimiter.limit(output, difference) &&
(((difference < 0) && (getKI() * getIntegral().getY() < 0)) ||
((difference > 0) && (getKI() * getIntegral().getY() > 0)))) {
getIntegral().setY(integralYPrevious);
}
return output;
}
private:
BlockIntegral _integral;
BlockParamFloat _kFF;
BlockParamFloat _kP;
BlockParamFloat _kI;
BlockParamFloat _offset;
};
/* A combination of feed forward, P and I gain using the output limiter with the option to provide a special output limiter (for example for takeoff)*/
class BlockFFPILimitedCustom: public BlockFFPILimited
{
public:
// methods
BlockFFPILimitedCustom(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
BlockFFPILimited(parent, name, isAngularLimit)
{};
virtual ~BlockFFPILimitedCustom() {};
float update(float inputValue, float inputError, BlockOutputLimiter *outputLimiter = NULL) {
return calcLimitedOutput(inputValue, inputError, outputLimiter == NULL ? _outputLimiter : *outputLimiter);
}
};
/* A combination of P gain and output limiter */
class BlockPLimited: public SuperBlock
{
public:
// methods
BlockPLimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
SuperBlock(parent, name),
_kP(this, "P"),
_outputLimiter(this, "", isAngularLimit)
{};
virtual ~BlockPLimited() {};
float update(float input) {
float difference = 0.0f;
float output = getKP() * input;
getOutputLimiter().limit(output, difference);
return output;
}
// accessors
BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; };
float getKP() { return _kP.get(); }
private:
control::BlockParamFloat _kP;
BlockOutputLimiter _outputLimiter;
};
/* A combination of P, D gains and output limiter */
class BlockPDLimited: public SuperBlock
{
public:
// methods
BlockPDLimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
SuperBlock(parent, name),
_kP(this, "P"),
_kD(this, "D"),
_derivative(this, "D"),
_outputLimiter(this, "", isAngularLimit)
{};
virtual ~BlockPDLimited() {};
float update(float input) {
float difference = 0.0f;
float output = getKP() * input + (getDerivative().getDt() > 0.0f ? getKD() * getDerivative().update(input) : 0.0f);
getOutputLimiter().limit(output, difference);
return output;
}
// accessors
float getKP() { return _kP.get(); }
float getKD() { return _kD.get(); }
BlockDerivative &getDerivative() { return _derivative; }
BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; };
private:
control::BlockParamFloat _kP;
control::BlockParamFloat _kD;
BlockDerivative _derivative;
BlockOutputLimiter _outputLimiter;
};
}
src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
+412
View File
@@ -0,0 +1,412 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@gmail.com>
*
* 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 mTecs_params.c
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/*
* Controller parameters, accessible via MAVLink
*/
/**
* mTECS enabled
*
* Set to 1 to enable mTECS
*
* @min 0
* @max 1
* @group mTECS
*/
PARAM_DEFINE_INT32(MT_ENABLED, 1);
/**
* Total Energy Rate Control Feedforward
* Maps the total energy rate setpoint to the throttle setpoint
*
* @min 0.0
* @max 10.0
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_THR_FF, 0.2f);
/**
* Total Energy Rate Control P
* Maps the total energy rate error to the throttle setpoint
*
* @min 0.0
* @max 10.0
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_THR_P, 0.03f);
/**
* Total Energy Rate Control I
* Maps the integrated total energy rate to the throttle setpoint
*
* @min 0.0
* @max 10.0
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_THR_I, 0.1f);
/**
* Total Energy Rate Control Offset (Cruise throttle sp)
*
* @min 0.0
* @max 10.0
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_THR_OFF, 0.7f);
/**
* Energy Distribution Rate Control Feedforward
* Maps the energy distribution rate setpoint to the pitch setpoint
*
* @min 0.0
* @max 10.0
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_PIT_FF, 0.1f);
/**
* Energy Distribution Rate Control P
* Maps the energy distribution rate error to the pitch setpoint
*
* @min 0.0
* @max 10.0
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_PIT_P, 0.03f);
/**
* Energy Distribution Rate Control I
* Maps the integrated energy distribution rate error to the pitch setpoint
*
* @min 0.0
* @max 10.0
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_PIT_I, 0.03f);
/**
* Total Energy Distribution Offset (Cruise pitch sp)
*
* @min 0.0
* @max 10.0
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_PIT_OFF, 0.0f);
/**
* Minimal Throttle Setpoint
*
* @min 0.0
* @max 1.0
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_THR_MIN, 0.0f);
/**
* Maximal Throttle Setpoint
*
* @min 0.0
* @max 1.0
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_THR_MAX, 1.0f);
/**
* Minimal Pitch Setpoint in Degrees
*
* @min -90.0f
* @max 90.0f
* @unit deg
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f);
/**
* Maximal Pitch Setpoint in Degrees
*
* @min -90.0f
* @max 90.0f
* @unit deg
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f);
/**
* P gain for the altitude control
* Maps the altitude error to the flight path angle setpoint
*
* @min 0.0f
* @max 10.0f
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_FPA_P, 0.2f);
/**
* D gain for the altitude control
* Maps the change of altitude error to the flight path angle setpoint
*
* @min 0.0f
* @max 10.0f
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_FPA_D, 0.0f);
/**
* Lowpass for FPA error derivative calculation (see MT_FPA_D)
*
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_FPA_D_LP, 1.0f);
/**
* Minimal flight path angle setpoint
*
* @min -90.0f
* @max 90.0f
* @unit deg
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_FPA_MIN, -10.0f);
/**
* Maximal flight path angle setpoint
*
* @min -90.0f
* @max 90.0f
* @unit deg
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f);
/**
* Lowpass (cutoff freq.) for airspeed
*
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_A_LP, 1.0f);
/**
* P gain for the airspeed control
* Maps the airspeed error to the acceleration setpoint
*
* @min 0.0f
* @max 10.0f
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_ACC_P, 1.5f);
/**
* D gain for the airspeed control
* Maps the change of airspeed error to the acceleration setpoint
*
* @min 0.0f
* @max 10.0f
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_ACC_D, 0.0f);
/**
* Lowpass for ACC error derivative calculation (see MT_ACC_D)
*
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 1.0f);
/**
* Minimal acceleration (air)
*
* @unit m/s^2
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_ACC_MIN, -40.0f);
/**
* Maximal acceleration (air)
*
* @unit m/s^2
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f);
/**
* Airspeed derivative calculation lowpass
*
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_AD_LP, 1.0f);
/**
* Minimal throttle during takeoff
*
* @min 0.0f
* @max 1.0f
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_TKF_THR_MIN, 1.0f);
/**
* Maximal throttle during takeoff
*
* @min 0.0f
* @max 1.0f
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_TKF_THR_MAX, 1.0f);
/**
* Minimal pitch during takeoff
*
* @min -90.0f
* @max 90.0f
* @unit deg
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_TKF_PIT_MIN, 0.0f);
/**
* Maximal pitch during takeoff
*
* @min -90.0f
* @max 90.0f
* @unit deg
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_TKF_PIT_MAX, 45.0f);
/**
* Minimal throttle in underspeed mode
*
* @min 0.0f
* @max 1.0f
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_USP_THR_MIN, 1.0f);
/**
* Maximal throttle in underspeed mode
*
* @min 0.0f
* @max 1.0f
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_USP_THR_MAX, 1.0f);
/**
* Minimal pitch in underspeed mode
*
* @min -90.0f
* @max 90.0f
* @unit deg
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_USP_PIT_MIN, -45.0f);
/**
* Maximal pitch in underspeed mode
*
* @min -90.0f
* @max 90.0f
* @unit deg
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_USP_PIT_MAX, 0.0f);
/**
* Minimal throttle in landing mode (only last phase of landing)
*
* @min 0.0f
* @max 1.0f
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_LND_THR_MIN, 0.0f);
/**
* Maximal throttle in landing mode (only last phase of landing)
*
* @min 0.0f
* @max 1.0f
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_LND_THR_MAX, 0.0f);
/**
* Minimal pitch in landing mode
*
* @min -90.0f
* @max 90.0f
* @unit deg
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_LND_PIT_MIN, -5.0f);
/**
* Maximal pitch in landing mode
*
* @min -90.0f
* @max 90.0f
* @unit deg
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_LND_PIT_MAX, 15.0f);
/**
* Integrator Limit for Total Energy Rate Control
*
* @min 0.0f
* @max 10.0f
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_THR_I_MAX, 10.0f);
/**
* Integrator Limit for Energy Distribution Rate Control
*
* @min 0.0f
* @max 10.0f
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_PIT_I_MAX, 10.0f);
src/modules/mavlink/mavlink_main.cpp
+12 -1
View File
@@ -899,7 +899,10 @@ 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_repeat_count = mavlink_mission_item->param2;
break;
default:
mission_item->acceptance_radius = mavlink_mission_item->param2;
mission_item->time_inside = mavlink_mission_item->param1;
@@ -915,6 +918,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;
}
@@ -932,6 +938,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
+55 -34
View File
@@ -120,50 +120,71 @@ 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->set_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:
case NAVIGATION_STATE_AUTO_RTL_RC:
case NAVIGATION_STATE_AUTO_RTL_DL:
*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_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;
@@ -603,8 +624,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
+3 -3
View File
@@ -667,12 +667,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/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
+80
View File
@@ -0,0 +1,80 @@
/****************************************************************************
*
* 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) :
NavigatorMode(navigator, name),
MissionBlock(navigator)
{
/* load initial params */
updateParams();
/* initial reset */
reset();
}
Loiter::~Loiter()
{
}
bool
Loiter::update(struct position_setpoint_triplet_s *pos_sp_triplet)
{
/* set loiter item, don't reuse an existing position setpoint */
return set_loiter_item(false, pos_sp_triplet);;
}
void
Loiter::reset()
{
}
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 NavigatorMode, MissionBlock
{
public:
/**
* Constructor
*/
Loiter(Navigator *navigator, const char *name);
/**
* Destructor
*/
~Loiter();
/**
* This function is called while the mode is inactive
*/
bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
/**
* This function is called while the mode is active
*/
void reset();
};
#endif
src/modules/navigator/mission.cpp
+472
View File
@@ -0,0 +1,472 @@
/****************************************************************************
*
* 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) :
NavigatorMode(navigator, name),
MissionBlock(navigator),
_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 */
reset();
}
Mission::~Mission()
{
}
void
Mission::reset()
{
_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::update(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->current, &pos_sp_triplet->previous);
/* 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_is_in_loiter(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_is_in_loiter(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;
bool use_current_pos_sp = pos_sp_triplet->current.valid && _waypoint_position_reached;
set_loiter_item(use_current_pos_sp, pos_sp_triplet);
reset_mission_item_reached();
report_mission_finished();
}
}
void
Mission::set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp,
struct position_setpoint_s *previous_pos_sp)
{
/* reuse current setpoint as previous setpoint */
if (current_pos_sp->valid) {
memcpy(previous_pos_sp, current_pos_sp, sizeof(struct position_setpoint_s));
}
}
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
+184
View File
@@ -0,0 +1,184 @@
/****************************************************************************
*
* 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 NavigatorMode, MissionBlock
{
public:
/**
* Constructor
*/
Mission(Navigator *navigator, const char *name);
/**
* Destructor
*/
virtual ~Mission();
/**
* This function is called while the mode is inactive
*/
virtual void reset();
/**
* This function is called while the mode is active
*/
virtual bool update(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);
/**
* Set previous position setpoint
*/
void set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp,
struct position_setpoint_s *previous_pos_sp);
/**
* 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
+225
View File
@@ -0,0 +1,225 @@
/****************************************************************************
*
* 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) :
_waypoint_position_reached(false),
_waypoint_yaw_reached(false),
_time_first_inside_orbit(0),
_mission_item({0}),
_mission_item_valid(false),
_navigator_priv(navigator)
{
}
MissionBlock::~MissionBlock()
{
}
bool
MissionBlock::is_mission_item_reached()
{
/* don't check landed WPs */
if (_mission_item.nav_cmd == NAV_CMD_LAND) {
return false;
}
/* TODO: count turns */
#if 0
if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
_mission_item.loiter_radius > 0.01f) {
return false;
}
#endif
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_priv->get_home_position()->alt
: _mission_item.altitude;
dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
_navigator_priv->get_global_position()->lat,
_navigator_priv->get_global_position()->lon,
_navigator_priv->get_global_position()->alt,
&dist_xy, &dist_z);
if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator_priv->get_vstatus()->is_rotary_wing) {
/* require only altitude for takeoff for multicopter */
if (_navigator_priv->get_global_position()->alt >
altitude_amsl - _navigator_priv->get_takeoff_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_priv->get_takeoff_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_priv->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_priv->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_priv->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;
if (item->nav_cmd == NAV_CMD_TAKEOFF) {
sp->type = SETPOINT_TYPE_TAKEOFF;
} else if (item->nav_cmd == NAV_CMD_LAND) {
sp->type = SETPOINT_TYPE_LAND;
} else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
sp->type = SETPOINT_TYPE_LOITER;
} else {
sp->type = SETPOINT_TYPE_POSITION;
}
}
bool
MissionBlock::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet)
{
if (_navigator_priv->get_is_in_loiter()) {
/* already loitering, bail out */
return false;
}
if (reuse_current_pos_sp && pos_sp_triplet->current.valid) {
/* leave position setpoint as is */
} else {
/* use current position */
pos_sp_triplet->current.lat = _navigator_priv->get_global_position()->lat;
pos_sp_triplet->current.lon = _navigator_priv->get_global_position()->lon;
pos_sp_triplet->current.alt = _navigator_priv->get_global_position()->alt;
pos_sp_triplet->current.yaw = NAN; /* NAN means to use current yaw */
}
pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
pos_sp_triplet->current.loiter_radius = _navigator_priv->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;
_navigator_priv->set_is_in_loiter(true);
return true;
}
src/modules/navigator/navigator_mission.h → src/modules/navigator/mission_block.h
+48 -48
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,75 @@
*
****************************************************************************/
/**
* @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>
class Navigator;
class __EXPORT Mission
class MissionBlock
{
public:
/**
* Constructor
*
* @param pointer to parent class
*/
Mission();
MissionBlock(Navigator *navigator);
/**
* 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 a loiter item, if possible reuse the position setpoint, otherwise take the current position
*
* @param true if the current position setpoint should be re-used
* @param the position setpoint triplet to set
* @return true if setpoint has changed
*/
bool set_loiter_item(const bool reuse_current_pos_sp, 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);
bool _waypoint_position_reached;
bool _waypoint_yaw_reached;
hrt_abstime _time_first_inside_orbit;
void move_to_next();
void report_mission_item_reached();
void report_current_offboard_mission_item();
void publish_mission_result();
mission_item_s _mission_item;
bool _mission_item_valid;
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;
Navigator *_navigator_priv;
};
#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
+211
View File
@@ -0,0 +1,211 @@
/***************************************************************************
*
* 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"
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_is_in_loiter(bool is_in_loiter) { _is_in_loiter = is_in_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_is_in_loiter() { return _is_in_loiter; }
float get_loiter_radius() { return _param_loiter_radius.get(); }
float get_takeoff_acceptance_radius() { return _param_takeoff_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 */
bool _is_in_loiter; /**< 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_takeoff_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
+290 -1130
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 */
reset();
}
NavigatorMode::~NavigatorMode()
{
}
void
NavigatorMode::reset()
{
_first_run = true;
}
bool
NavigatorMode::update(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 reset();
/**
* 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 update(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
* Takeoff Acceptance Radius (FW only)
*
* @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.
* Acceptance radius for fixedwing.
*
* @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_TF_ACC_RAD, 25.0f);
src/modules/navigator/rtl.cpp
+282
View File
@@ -0,0 +1,282 @@
/****************************************************************************
*
* 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 <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/home_position.h>
#include "navigator.h"
#include "rtl.h"
RTL::RTL(Navigator *navigator, const char *name) :
NavigatorMode(navigator, name),
MissionBlock(navigator),
_rtl_state(RTL_STATE_NONE),
_param_return_alt(this, "RETURN_ALT"),
_param_descend_alt(this, "DESCEND_ALT"),
_param_land_delay(this, "LAND_DELAY"),
_param_acceptance_radius(this, "ACCEPT_RAD")
{
/* load initial params */
updateParams();
/* initial reset */
reset();
}
RTL::~RTL()
{
}
void
RTL::reset()
{
_first_run = true;
_rtl_state = RTL_STATE_NONE;
}
bool
RTL::update(struct position_setpoint_triplet_s *pos_sp_triplet)
{
bool updated = false;
if (_first_run) {
set_rtl_item(pos_sp_triplet);
updated = true;
_first_run = false;
}
if ((_rtl_state == RTL_STATE_CLIMB
|| _rtl_state == RTL_STATE_RETURN)
&& 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();
/* 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_FINISHED;
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 {
_rtl_state = RTL_STATE_RETURN;
}
}
/* if switching directly to return state, set altitude setpoint to current altitude */
if (_rtl_state == RTL_STATE_RETURN) {
_mission_item.altitude_is_relative = false;
_mission_item.altitude = _navigator->get_global_position()->alt;
}
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 = _param_acceptance_radius.get();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_is_in_loiter(false);
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;
/* TODO: add this again */
// 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(_global_pos.lat, _global_pos.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 = _param_acceptance_radius.get();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_is_in_loiter(false);
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 = _param_acceptance_radius.get();
_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 = _param_land_delay.get() > -0.001f;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_is_in_loiter(true);
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_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 = _param_acceptance_radius.get();
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
_navigator->set_is_in_loiter(false);
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home");
break;
}
case RTL_STATE_FINISHED: {
/* nothing to do, report fail */
}
default:
break;
}
if (_rtl_state == RTL_STATE_FINISHED) {
pos_sp_triplet->current.valid = false;
pos_sp_triplet->next.valid = false;
} else {
/* if not finished, 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() < 0) {
_rtl_state = RTL_STATE_FINISHED;
} else {
_rtl_state = RTL_STATE_LAND;
}
break;
case RTL_STATE_LAND:
_rtl_state = RTL_STATE_FINISHED;
break;
case RTL_STATE_FINISHED:
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 NavigatorMode, MissionBlock
{
public:
/**
* Constructor
*/
RTL(Navigator *navigator, const char *name);
/**
* Destructor
*/
~RTL();
/**
* This function is called while the mode is inactive
*/
void reset();
/**
* This function is called while the mode is active
*
* @param position setpoint triplet that needs to be set
* @return true if updated
*/
bool update(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_LAND,
RTL_STATE_FINISHED,
} _rtl_state;
control::BlockParamFloat _param_return_alt;
control::BlockParamFloat _param_descend_alt;
control::BlockParamFloat _param_land_delay;
control::BlockParamFloat _param_acceptance_radius;
};
#endif
src/modules/navigator/rtl_params.c
+110
View File
@@ -0,0 +1,110 @@
/****************************************************************************
*
* 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);
/**
* RTL acceptance radius
*
* Acceptance radius for waypoints set for RTL
*
* @unit meters
* @min 1
* @max
* @group RTL
*/
PARAM_DEFINE_FLOAT(RTL_ACCEPT_RAD, 25.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
+45 -3
View File
@@ -84,8 +84,10 @@
#include <uORB/topics/esc_status.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/tecs_status.h>
#include <uORB/topics/system_power.h>
#include <uORB/topics/servorail_status.h>
#include <uORB/topics/wind_estimate.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
@@ -939,8 +941,10 @@ int sdlog2_thread_main(int argc, char *argv[])
struct telemetry_status_s telemetry;
struct range_finder_report range_finder;
struct estimator_status_report estimator_status;
struct tecs_status_s tecs_status;
struct system_power_s system_power;
struct servorail_status_s servorail_status;
struct wind_estimate_s wind_estimate;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -979,6 +983,8 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_GS0B_s log_GS0B;
struct log_GS1A_s log_GS1A;
struct log_GS1B_s log_GS1B;
struct log_TECS_s log_TECS;
struct log_WIND_s log_WIND;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@@ -1010,8 +1016,10 @@ int sdlog2_thread_main(int argc, char *argv[])
int telemetry_sub;
int range_finder_sub;
int estimator_status_sub;
int tecs_status_sub;
int system_power_sub;
int servorail_status_sub;
int wind_sub;
} subs;
subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
@@ -1037,8 +1045,12 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status));
subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));
subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate));
/* we need to rate-limit wind, as we do not need the full update rate */
orb_set_interval(subs.wind_sub, 90);
thread_running = true;
@@ -1126,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;
@@ -1340,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;
@@ -1488,6 +1500,36 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(ESTM);
}
/* --- TECS STATUS --- */
if (copy_if_updated(ORB_ID(tecs_status), subs.tecs_status_sub, &buf.tecs_status)) {
log_msg.msg_type = LOG_TECS_MSG;
log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp;
log_msg.body.log_TECS.altitude = buf.tecs_status.altitude;
log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp;
log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp;
log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed;
log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered;
log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp;
log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative;
log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp;
log_msg.body.log_TECS.totalEnergyRate = buf.tecs_status.totalEnergyRate;
log_msg.body.log_TECS.energyDistributionRateSp = buf.tecs_status.energyDistributionRateSp;
log_msg.body.log_TECS.energyDistributionRate = buf.tecs_status.energyDistributionRate;
log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode;
LOGBUFFER_WRITE_AND_COUNT(TECS);
}
/* --- WIND ESTIMATE --- */
if (copy_if_updated(ORB_ID(wind_estimate), subs.wind_sub, &buf.wind_estimate)) {
log_msg.msg_type = LOG_WIND_MSG;
log_msg.body.log_WIND.x = buf.wind_estimate.windspeed_north;
log_msg.body.log_WIND.y = buf.wind_estimate.windspeed_east;
log_msg.body.log_WIND.cov_x = buf.wind_estimate.covariance_north;
log_msg.body.log_WIND.cov_y = buf.wind_estimate.covariance_east;
LOGBUFFER_WRITE_AND_COUNT(WIND);
}
/* signal the other thread new data, but not yet unlock */
if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
/* only request write if several packets can be written at once */
src/modules/sdlog2/sdlog2_messages.h
+32
View File
@@ -346,6 +346,36 @@ struct log_GS1B_s {
uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
};
/* --- TECS - TECS STATUS --- */
#define LOG_TECS_MSG 30
struct log_TECS_s {
float altitudeSp;
float altitude;
float flightPathAngleSp;
float flightPathAngle;
float airspeedSp;
float airspeed;
float airspeedFiltered;
float airspeedDerivativeSp;
float airspeedDerivative;
float totalEnergyRateSp;
float totalEnergyRate;
float energyDistributionRateSp;
float energyDistributionRate;
uint8_t mode;
};
/* --- WIND - WIND ESTIMATE --- */
#define LOG_WIND_MSG 31
struct log_WIND_s {
float x;
float y;
float cov_x;
float cov_y;
};
/********** SYSTEM MESSAGES, ID > 0x80 **********/
/* --- TIME - TIME STAMP --- */
@@ -401,6 +431,8 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(TECS, "fffffffffffffB", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,Mod"),
LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"),
/* system-level messages, ID >= 0x80 */
/* FMT: don't write format of format message, it's useless */
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/Publication.cpp
+2
View File
@@ -47,6 +47,7 @@
#include "topics/vehicle_rates_setpoint.h"
#include "topics/actuator_outputs.h"
#include "topics/encoders.h"
#include "topics/tecs_status.h"
namespace uORB {
@@ -76,5 +77,6 @@ template class __EXPORT Publication<vehicle_attitude_setpoint_s>;
template class __EXPORT Publication<vehicle_rates_setpoint_s>;
template class __EXPORT Publication<actuator_outputs_s>;
template class __EXPORT Publication<encoders_s>;
template class __EXPORT Publication<tecs_status_s>;
}
src/modules/uORB/objects_common.cpp
+6
View File
@@ -199,3 +199,9 @@ ORB_DEFINE(encoders, struct encoders_s);
#include "topics/estimator_status.h"
ORB_DEFINE(estimator_status, struct estimator_status_report);
#include "topics/tecs_status.h"
ORB_DEFINE(tecs_status, struct tecs_status_s);
#include "topics/wind_estimate.h"
ORB_DEFINE(wind_estimate, struct wind_estimate_s);
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
+8 -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_
@@ -58,7 +58,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 +92,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/tecs_status.h
+92
View File
@@ -0,0 +1,92 @@
/****************************************************************************
*
* 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
* 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 vehicle_global_position.h
* Definition of the global fused WGS84 position uORB topic.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
*/
#ifndef TECS_STATUS_T_H_
#define TECS_STATUS_T_H_
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
/**
* @addtogroup topics
* @{
*/
typedef enum {
TECS_MODE_NORMAL,
TECS_MODE_UNDERSPEED,
TECS_MODE_TAKEOFF,
TECS_MODE_LAND,
TECS_MODE_LAND_THROTTLELIM
} tecs_mode;
/**
* Internal values of the (m)TECS fixed wing speed alnd altitude control system
*/
struct tecs_status_s {
uint64_t timestamp; /**< timestamp, in microseconds since system start */
float altitudeSp;
float altitude;
float flightPathAngleSp;
float flightPathAngle;
float airspeedSp;
float airspeed;
float airspeedFiltered;
float airspeedDerivativeSp;
float airspeedDerivative;
float totalEnergyRateSp;
float totalEnergyRate;
float energyDistributionRateSp;
float energyDistributionRate;
tecs_mode mode;
};
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(tecs_status);
#endif
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
+26 -12
View File
@@ -54,8 +54,6 @@
#include <stdbool.h>
#include "../uORB.h"
#include <navigator/navigator_state.h>
/**
* @addtogroup topics @{
*/
@@ -65,9 +63,11 @@ 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 +80,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 {
@@ -90,12 +90,27 @@ typedef enum {
typedef enum {
FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */
FAILSAFE_STATE_RTL, /**< Return To Launch */
FAILSAFE_STATE_LAND, /**< Land without position control */
FAILSAFE_STATE_RTL_RC, /**< Return To Launch on remote control loss */
FAILSAFE_STATE_RTL_DL, /**< Return To Launch on datalink loss */
FAILSAFE_STATE_LAND, /**< Land as safe as possible */
FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */
FAILSAFE_STATE_MAX
FAILSAFE_STATE_MAX,
} failsafe_state_t;
typedef enum {
NAVIGATION_STATE_MANUAL = 0,
NAVIGATION_STATE_ACRO,
NAVIGATION_STATE_ALTCTL,
NAVIGATION_STATE_POSCTL,
NAVIGATION_STATE_AUTO_MISSION,
NAVIGATION_STATE_AUTO_LOITER,
NAVIGATION_STATE_AUTO_RTL,
NAVIGATION_STATE_AUTO_RTL_RC,
NAVIGATION_STATE_AUTO_RTL_DL,
NAVIGATION_STATE_LAND,
NAVIGATION_STATE_TERMINATION,
} navigation_state_t;
enum VEHICLE_MODE_FLAG {
VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64,
@@ -154,11 +169,10 @@ 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 set_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 */
hil_state_t hil_state; /**< current hil state */
failsafe_state_t failsafe_state; /**< current failsafe state */
int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */
src/modules/uORB/topics/wind_estimate.h
+68
View File
@@ -0,0 +1,68 @@
/****************************************************************************
*
* 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 wind_estimate.h
*
* Wind estimate topic topic
*
*/
#ifndef TOPIC_WIND_ESTIMATE_H
#define TOPIC_WIND_ESTIMATE_H
#include <stdint.h>
#include "../uORB.h"
/**
* @addtogroup topics
* @{
*/
/** Wind estimate */
struct wind_estimate_s {
uint64_t timestamp; /**< Microseconds since system boot */
float windspeed_north; /**< Wind component in north / X direction */
float windspeed_east; /**< Wind component in east / Y direction */
float covariance_north; /**< Uncertainty - set to zero (no uncertainty) if not estimated */
float covariance_east; /**< Uncertainty - set to zero (no uncertainty) if not estimated */
};
/**
* @}
*/
ORB_DECLARE(wind_estimate);
#endif