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

Merge pull request #1089 from PX4/navigator_rewrite

Browse Source 
Navigator rewrite
This commit is contained in:
Lorenz Meier
2014-06-29 19:10:42 +02:00
parent 9388b04557 5db51bd704
commit 94c69e11ae
47 changed files with 2986 additions and 2591 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/uORB/topics/home_position.h
+4 -1
View File
@@ -59,10 +59,13 @@ struct home_position_s
{
uint64_t timestamp; /**< Timestamp (microseconds since system boot) */
//bool altitude_is_relative; // TODO what means home relative altitude? we need clear definition of reference altitude then
double lat; /**< Latitude in degrees */
double lon; /**< Longitude in degrees */
float alt; /**< Altitude in meters */
float x; /**< X coordinate in meters */
float y; /**< Y coordinate in meters */
float z; /**< Z coordinate in meters */
};
/**
src/modules/uORB/topics/mission.h
+9 -4
View File
@@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -37,6 +34,9 @@
/**
* @file mission.h
* Definition of a mission consisting of mission items.
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#ifndef TOPIC_MISSION_H_
@@ -50,6 +50,7 @@
/* compatible to mavlink MAV_CMD */
enum NAV_CMD {
NAV_CMD_IDLE=0,
NAV_CMD_WAYPOINT=16,
NAV_CMD_LOITER_UNLIMITED=17,
NAV_CMD_LOITER_TURN_COUNT=18,
@@ -58,7 +59,8 @@ enum NAV_CMD {
NAV_CMD_LAND=21,
NAV_CMD_TAKEOFF=22,
NAV_CMD_ROI=80,
NAV_CMD_PATHPLANNING=81
NAV_CMD_PATHPLANNING=81,
NAV_CMD_DO_JUMP=177
};
enum ORIGIN {
@@ -91,6 +93,9 @@ struct mission_item_s {
float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */
bool autocontinue; /**< true if next waypoint should follow after this one */
enum ORIGIN origin; /**< where the waypoint has been generated */
int do_jump_mission_index; /**< index where the do jump will go to */
unsigned do_jump_repeat_count; /**< how many times do jump needs to be done */
unsigned do_jump_current_count; /**< count how many times the jump has been done */
};
struct mission_s
src/modules/uORB/topics/mission_result.h
+1
View File
@@ -56,6 +56,7 @@ struct mission_result_s
bool mission_reached; /**< true if mission has been reached */
unsigned mission_index_reached; /**< index of the mission which has been reached */
unsigned index_current_mission; /**< index of the current mission */
bool mission_finished; /**< true if mission has been completed */
};
/**
src/modules/uORB/topics/position_setpoint_triplet.h
+10 -11
View File
@@ -1,9 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -37,6 +34,10 @@
/**
* @file mission_item_triplet.h
* Definition of the global WGS84 position setpoint uORB topic.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#ifndef TOPIC_MISSION_ITEM_TRIPLET_H_
@@ -45,7 +46,6 @@
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
#include <navigator/navigator_state.h>
/**
* @addtogroup topics
@@ -54,11 +54,12 @@
enum SETPOINT_TYPE
{
SETPOINT_TYPE_NORMAL = 0, /**< normal setpoint */
SETPOINT_TYPE_LOITER, /**< loiter setpoint */
SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */
SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, vehicle must descend until landing */
SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */
SETPOINT_TYPE_POSITION = 0, /**< position setpoint */
SETPOINT_TYPE_VELOCITY, /**< velocity setpoint */
SETPOINT_TYPE_LOITER, /**< loiter setpoint */
SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */
SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, descend until landing */
SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */
};
struct position_setpoint_s
@@ -84,8 +85,6 @@ struct position_setpoint_triplet_s
struct position_setpoint_s previous;
struct position_setpoint_s current;
struct position_setpoint_s next;
nav_state_t nav_state; /**< navigation state */
};
/**
src/modules/uORB/topics/telemetry_status.h
+1
View File
@@ -57,6 +57,7 @@ enum TELEMETRY_STATUS_RADIO_TYPE {
struct telemetry_status_s {
uint64_t timestamp;
uint64_t heartbeat_time; /**< Time of last received heartbeat from remote system */
enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */
uint8_t rssi; /**< local signal strength */
uint8_t remote_rssi; /**< remote signal strength */
src/modules/uORB/topics/vehicle_global_position.h
+6 -7
View File
@@ -36,7 +36,7 @@
* Definition of the global fused WGS84 position uORB topic.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
@@ -61,15 +61,14 @@
* e.g. control inputs of the vehicle in a Kalman-filter implementation.
*/
struct vehicle_global_position_s {
uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */
uint64_t time_gps_usec; /**< GPS timestamp in microseconds */
uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */
uint64_t time_gps_usec; /**< GPS timestamp in microseconds */
double lat; /**< Latitude in degrees */
double lon; /**< Longitude in degrees */
float alt; /**< Altitude AMSL in meters */
float vel_n; /**< Ground north velocity, m/s */
float vel_e; /**< Ground east velocity, m/s */
float vel_d; /**< Ground downside velocity, m/s */
float vel_n; /**< Ground north velocity, m/s */
float vel_e; /**< Ground east velocity, m/s */
float vel_d; /**< Ground downside velocity, m/s */
float yaw; /**< Yaw in radians -PI..+PI. */
float eph;
float epv;
src/modules/uORB/topics/vehicle_gps_position.h
+2 -2
View File
@@ -65,8 +65,8 @@ struct vehicle_gps_position_s {
float c_variance_rad; /**< course accuracy estimate rad */
uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */
float eph_m; /**< GPS HDOP horizontal dilution of position in m */
float epv_m; /**< GPS VDOP horizontal dilution of position in m */
float eph; /**< GPS HDOP horizontal dilution of position in m */
float epv; /**< GPS VDOP horizontal dilution of position in m */
unsigned noise_per_ms; /**< */
unsigned jamming_indicator; /**< */
src/modules/uORB/topics/vehicle_status.h
+36 -22
View File
@@ -1,10 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Lorenz Meier <lm@inf.ethz.ch>
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* Copyright (C) 2012 - 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -45,6 +41,11 @@
* All apps should write to subsystem_info:
*
* (any app) --> subsystem_info (published) --> (commander app state machine) --> vehicle_status --> (mavlink app)
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <julian@oes.ch>
*/
#ifndef VEHICLE_STATUS_H_
@@ -54,20 +55,22 @@
#include <stdbool.h>
#include "../uORB.h"
#include <navigator/navigator_state.h>
/**
* @addtogroup topics @{
*/
/* main state machine */
/**
* Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link.
*/
typedef enum {
MAIN_STATE_MANUAL = 0,
MAIN_STATE_ALTCTL,
MAIN_STATE_POSCTL,
MAIN_STATE_AUTO,
MAIN_STATE_AUTO_MISSION,
MAIN_STATE_AUTO_LOITER,
MAIN_STATE_AUTO_RTL,
MAIN_STATE_ACRO,
MAIN_STATE_MAX
MAIN_STATE_MAX,
} main_state_t;
// If you change the order, add or remove arming_state_t states make sure to update the arrays
@@ -80,7 +83,7 @@ typedef enum {
ARMING_STATE_STANDBY_ERROR,
ARMING_STATE_REBOOT,
ARMING_STATE_IN_AIR_RESTORE,
ARMING_STATE_MAX
ARMING_STATE_MAX,
} arming_state_t;
typedef enum {
@@ -88,13 +91,23 @@ typedef enum {
HIL_STATE_ON
} hil_state_t;
/**
* Navigation state, i.e. "what should vehicle do".
*/
typedef enum {
FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */
FAILSAFE_STATE_RTL, /**< Return To Launch */
FAILSAFE_STATE_LAND, /**< Land without position control */
FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */
FAILSAFE_STATE_MAX
} failsafe_state_t;
NAVIGATION_STATE_MANUAL = 0, /**< Manual mode */
NAVIGATION_STATE_ALTCTL, /**< Altitude control mode */
NAVIGATION_STATE_POSCTL, /**< Position control mode */
NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */
NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */
NAVIGATION_STATE_AUTO_RTL, /**< Auto return to launch mode */
NAVIGATION_STATE_AUTO_RTGS, /**< Auto return to groundstation on data link loss */
NAVIGATION_STATE_ACRO, /**< Acro mode */
NAVIGATION_STATE_LAND, /**< Land mode */
NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */
NAVIGATION_STATE_TERMINATION, /**< Termination mode */
NAVIGATION_STATE_MAX,
} navigation_state_t;
enum VEHICLE_MODE_FLAG {
VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
@@ -154,12 +167,11 @@ struct vehicle_status_s {
uint16_t counter; /**< incremented by the writing thread everytime new data is stored */
uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
main_state_t main_state; /**< main state machine */
unsigned int set_nav_state; /**< set navigation state machine to specified value */
uint64_t set_nav_state_timestamp; /**< timestamp of latest change of set_nav_state */
main_state_t main_state; /**< main state machine */
navigation_state_t nav_state; /**< set navigation state machine to specified value */
arming_state_t arming_state; /**< current arming state */
hil_state_t hil_state; /**< current hil state */
failsafe_state_t failsafe_state; /**< current failsafe state */
hil_state_t hil_state; /**< current hil state */
bool failsafe; /**< true if system is in failsafe state */
int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */
int32_t system_id; /**< system id, inspired by MAVLink's system ID field */
@@ -184,6 +196,8 @@ struct vehicle_status_s {
bool rc_signal_lost; /**< true if RC reception lost */
bool rc_input_blocked; /**< set if RC input should be ignored */
bool data_link_lost; /**< datalink to GCS lost */
bool offboard_control_signal_found_once;
bool offboard_control_signal_lost;
bool offboard_control_signal_weak;