dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #3063 from PX4/home_on_takeoff 

Home on takeoff
This commit is contained in:
Roman Bapst 2015-10-31 10:42:51 +01:00
parent fe4c67a781 5759358da9
commit e7ce15ccd5
4 changed files with 45 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
msg/home_position.msg
View File

@ -8,3 +8,8 @@ float32 alt # Altitude in meters (AMSL)
float32 x # X coordinate in meters
float32 y # Y coordinate in meters
float32 z # Z coordinate in meters
float32 yaw # Yaw angle in radians
float32 direction_x # Takeoff direction in NED X
float32 direction_y # Takeoff direction in NED Y
float32 direction_z # Takeoff direction in NED Z

2
src/modules/commander/CMakeLists.txt
View File

@ -32,7 +32,7 @@
############################################################################
set(MODULE_CFLAGS -Os)
if(${OS} STREQUAL "nuttx")
list(APPEND MODULE_CFLAGS -Wframe-larger-than=2300)
list(APPEND MODULE_CFLAGS -Wframe-larger-than=2400)
endif()
px4_add_module(
MODULE modules__commander

60
src/modules/commander/commander.cpp
View File

@ -72,6 +72,7 @@
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_command.h>
@ -150,7 +151,7 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage
#define PRINT_INTERVAL 5000000
#define PRINT_MODE_REJECT_INTERVAL 10000000
#define INAIR_RESTART_HOLDOFF_INTERVAL 2000000
#define INAIR_RESTART_HOLDOFF_INTERVAL 1000000
#define HIL_ID_MIN 1000
#define HIL_ID_MAX 1999
@ -223,7 +224,7 @@ void usage(const char *reason);
*/
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,
struct vehicle_local_position_s *local_pos, orb_advert_t *home_pub);
struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub);
/**
* Mainloop of commander.
@ -258,7 +259,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armed
* time the vehicle is armed with a good GPS fix.
**/
static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home,
const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition);
const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition,
const vehicle_attitude_s &attitude);
/**
* Loop that runs at a lower rate and priority for calibration and parameter tasks.
@ -296,7 +298,7 @@ int commander_main(int argc, char *argv[])
daemon_task = px4_task_spawn_cmd("commander",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40,
3400,
3500,
commander_thread_main,
(char * const *)&argv[0]);
@ -495,7 +497,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local,
struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local,
struct home_position_s *home, struct vehicle_global_position_s *global_pos,
struct vehicle_local_position_s *local_pos, orb_advert_t *home_pub)
struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub)
{
/* only handle commands that are meant to be handled by this system and component */
if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id)
@ -529,7 +531,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
if (cmd_arm && (arming_ret == TRANSITION_CHANGED) &&
(hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL))) {
commander_set_home_position(*home_pub, *home, *local_pos, *global_pos);
commander_set_home_position(*home_pub, *home, *local_pos, *global_pos, *attitude);
}
if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
@ -838,7 +840,8 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
* time the vehicle is armed with a good GPS fix.
**/
static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home,
const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition)
const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition,
const vehicle_attitude_s &attitude)
{
//Need global position fix to be able to set home
if (!status.condition_global_position_valid) {
@ -860,6 +863,8 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s &
home.y = localPosition.y;
home.z = localPosition.z;
home.yaw = attitude.yaw;
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);
@ -886,6 +891,7 @@ int commander_thread_main(int argc, char *argv[])
commander_initialized = false;
bool arm_tune_played = false;
bool was_landed = true;
bool was_armed = false;
bool startup_in_hil = false;
@ -1037,17 +1043,15 @@ int commander_thread_main(int argc, char *argv[])
px4_task_exit(ERROR);
}
/* armed topic */
orb_advert_t armed_pub;
/* Initialize armed with all false */
memset(&armed, 0, sizeof(armed));
/* armed topic */
orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
/* vehicle control mode topic */
memset(&control_mode, 0, sizeof(control_mode));
orb_advert_t control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
/* home position */
orb_advert_t home_pub = nullptr;
memset(&_home, 0, sizeof(_home));
@ -1146,13 +1150,15 @@ int commander_thread_main(int argc, char *argv[])
/* Subscribe to local position data */
int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
struct vehicle_local_position_s local_position;
memset(&local_position, 0, sizeof(local_position));
struct vehicle_local_position_s local_position = {};
/* Subscribe to attitude data */
int attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
struct vehicle_attitude_s attitude = {};
/* Subscribe to land detector */
int land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
struct vehicle_land_detected_s land_detector;
memset(&land_detector, 0, sizeof(land_detector));
struct vehicle_land_detected_s land_detector = {};
/*
* The home position is set based on GPS only, to prevent a dependency between
@ -1609,6 +1615,14 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
}
/* update attitude estimate */
orb_check(attitude_sub, &updated);
if (updated) {
/* position changed */
orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &attitude);
}
//update condition_global_position_valid
//Global positions are only published by the estimators if they are valid
if (hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) {
@ -2239,7 +2253,7 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
/* handle it */
if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, &home_pub)) {
if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, &attitude, &home_pub)) {
status_changed = true;
}
}
@ -2303,14 +2317,18 @@ int commander_thread_main(int argc, char *argv[])
/* First time home position update - but only if disarmed */
if (!status.condition_home_position_valid && !armed.armed) {
commander_set_home_position(home_pub, _home, local_position, global_position);
commander_set_home_position(home_pub, _home, local_position, global_position, attitude);
}
/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) {
commander_set_home_position(home_pub, _home, local_position, global_position);
/* update home position on arming if at least 1s from commander start spent to avoid setting home on in-air restart */
else if (((!was_armed && armed.armed) || (was_landed && !status.condition_landed)) &&
(now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) {
commander_set_home_position(home_pub, _home, local_position, global_position, attitude);
}
was_landed = status.condition_landed;
was_armed = armed.armed;
/* print new state */
if (arming_state_changed) {
status_changed = true;
@ -2318,8 +2336,6 @@ int commander_thread_main(int argc, char *argv[])
arming_state_changed = false;
}
was_armed = armed.armed;
/* now set navigation state according to failsafe and main state */
bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled,
mission_result.finished,

2
src/modules/navigator/rtl.cpp
View File

@ -190,7 +190,7 @@ RTL::set_rtl_item()
_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.yaw = _navigator->get_home_position()->yaw;
_mission_item.loiter_radius = _navigator->get_loiter_radius();
_mission_item.loiter_direction = 1;
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;