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

allow local position for takeoff (e.g. flow)

Browse Source
This commit is contained in:
ChristophTobler 2017-02-06 16:48:37 +01:00 committed by Lorenz Meier
parent 194f0c1de8
commit 1f7fdb2386
4 changed files with 45 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -433,7 +433,7 @@ int commander_main(int argc, char *argv[])
if (!strcmp(argv[1], "takeoff")) {
/* see if we got a home position */
if (status_flags.condition_home_position_valid) {
if (status_flags.condition_local_position_valid) {
if (TRANSITION_DENIED != arm_disarm(true, &mavlink_log_pub, "command line")) {

18
src/modules/commander/state_machine_helper.cpp
View File

@ -428,8 +428,6 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
case commander_state_s::MAIN_STATE_AUTO_MISSION:
case commander_state_s::MAIN_STATE_AUTO_RTL:
case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:
case commander_state_s::MAIN_STATE_AUTO_LAND:
/* need global position and home position */
if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) {
@ -438,6 +436,16 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
break;
case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:
case commander_state_s::MAIN_STATE_AUTO_LAND:
/* need local position */
if (status_flags->condition_local_position_valid) {
ret = TRANSITION_CHANGED;
}
break;
case commander_state_s::MAIN_STATE_OFFBOARD:
/* need offboard signal
@ -937,8 +945,7 @@ bool set_nav_state(struct vehicle_status_s *status,
if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (status_flags->gps_failure || (!status_flags->condition_global_position_valid ||
!status_flags->condition_home_position_valid)) {
} else if (status_flags->gps_failure || !status_flags->condition_local_position_valid) {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps);
if (status_flags->condition_local_position_valid) {
@ -964,8 +971,7 @@ bool set_nav_state(struct vehicle_status_s *status,
if (status->engine_failure) {
status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (status_flags->gps_failure || (!status_flags->condition_global_position_valid ||
!status_flags->condition_home_position_valid)) {
} else if (status_flags->gps_failure || !status_flags->condition_local_position_valid) {
enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps);
if (status_flags->condition_local_altitude_valid) {

9
src/modules/navigator/navigator.h
View File

@ -52,6 +52,7 @@
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/mission_result.h>
@ -137,6 +138,7 @@ public:
struct vehicle_land_detected_s *get_land_detected() { return &_land_detected; }
struct vehicle_control_mode_s *get_control_mode() { return &_control_mode; }
struct vehicle_global_position_s *get_global_position() { return &_global_pos; }
struct vehicle_local_position_s *get_local_position() { return &_local_pos; }
struct vehicle_gps_position_s *get_gps_position() { return &_gps_pos; }
struct sensor_combined_s *get_sensor_combined() { return &_sensor_combined; }
struct home_position_s *get_home_position() { return &_home_pos; }
@ -238,6 +240,7 @@ private:
orb_advert_t _mavlink_log_pub; /**< the uORB advert to send messages over mavlink */
int _global_pos_sub; /**< global position subscription */
int _local_pos_sub; /**< local position subscription */
int _gps_pos_sub; /**< gps position subscription */
int _sensor_combined_sub; /**< sensor combined subscription */
int _home_pos_sub; /**< home position subscription */
@ -261,6 +264,7 @@ private:
vehicle_land_detected_s _land_detected; /**< vehicle land_detected */
vehicle_control_mode_s _control_mode; /**< vehicle control mode */
vehicle_global_position_s _global_pos; /**< global vehicle position */
vehicle_local_position_s _local_pos; /**< local vehicle position */
vehicle_gps_position_s _gps_pos; /**< gps position */
sensor_combined_s _sensor_combined; /**< sensor values */
home_position_s _home_pos; /**< home position for RTL */
@ -324,6 +328,11 @@ private:
*/
void global_position_update();
/**
* Retrieve local position
*/
void local_position_update();
/**
* Retrieve gps position
*/

25
src/modules/navigator/navigator_main.cpp
View File

@ -105,6 +105,7 @@ Navigator::Navigator() :
_navigator_task(-1),
_mavlink_log_pub(nullptr),
_global_pos_sub(-1),
_local_pos_sub(-1),
_gps_pos_sub(-1),
_sensor_combined_sub(-1),
_home_pos_sub(-1),
@ -212,6 +213,12 @@ Navigator::global_position_update()
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
}
void
Navigator::local_position_update()
{
orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
}
void
Navigator::gps_position_update()
{
@ -301,6 +308,7 @@ Navigator::task_main()
/* do subscriptions */
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
_fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status));
@ -318,6 +326,7 @@ Navigator::task_main()
vehicle_land_detected_update();
vehicle_control_mode_update();
global_position_update();
local_position_update();
gps_position_update();
sensor_combined_update();
home_position_update(true);
@ -382,6 +391,13 @@ Navigator::task_main()
}
}
/* local position updated */
orb_check(_local_pos_sub, &updated);
if (updated) {
local_position_update();
}
/* sensors combined updated */
orb_check(_sensor_combined_sub, &updated);
@ -492,7 +508,13 @@ Navigator::task_main()
rep->current.loiter_radius = get_loiter_radius();
rep->current.loiter_direction = 1;
rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
rep->current.yaw = cmd.param4;
if (home_position_valid()) {
rep->current.yaw = cmd.param4;
rep->previous.valid = true;
} else {
rep->current.yaw = get_local_position()->yaw;
rep->previous.valid = false;
}
if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7;
@ -506,7 +528,6 @@ Navigator::task_main()
rep->current.alt = cmd.param7;
rep->previous.valid = true;
rep->current.valid = true;
rep->next.valid = false;