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

Merge remote-tracking branch 'px4/pr/1058' into navigator_rewrite_offboard

Browse Source 
Conflicts:
	src/modules/commander/commander.cpp
	src/modules/commander/state_machine_helper.cpp
	src/modules/mavlink/mavlink_receiver.cpp
	src/modules/mavlink/mavlink_receiver.h
	src/modules/uORB/topics/vehicle_status.h
This commit is contained in:
Julian Oes 2014-06-20 18:18:39 +02:00
commit 4e08457afe
12 changed files with 294 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -124,6 +124,7 @@ extern struct system_load_s system_load;
#define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
#define RC_TIMEOUT 500000
#define DL_TIMEOUT 5 * 1000* 1000
#define OFFBOARD_TIMEOUT 500000
#define DIFFPRESS_TIMEOUT 2000000
#define PRINT_INTERVAL 5000000
@ -164,6 +165,7 @@ static struct vehicle_status_s status;
static struct actuator_armed_s armed;
static struct safety_s safety;
static struct vehicle_control_mode_s control_mode;
static struct offboard_control_setpoint_s sp_offboard;
/* tasks waiting for low prio thread */
typedef enum {
@ -437,6 +439,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
/* ACRO */
main_ret = main_state_transition(status, MAIN_STATE_ACRO);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
/* OFFBOARD */
main_ret = main_state_transition(status, MAIN_STATE_OFFBOARD);
}
} else {
@ -632,6 +638,7 @@ int commander_thread_main(int argc, char *argv[])
main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER";
main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL";
main_states_str[MAIN_STATE_ACRO] = "ACRO";
main_states_str[MAIN_STATE_OFFBOARD] = "OFFBOARD";
char *arming_states_str[ARMING_STATE_MAX];
arming_states_str[ARMING_STATE_INIT] = "INIT";
@ -778,7 +785,6 @@ int commander_thread_main(int argc, char *argv[])
/* Subscribe to offboard control data */
int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
struct offboard_control_setpoint_s sp_offboard;
memset(&sp_offboard, 0, sizeof(sp_offboard));
/* Subscribe to telemetry status */
@ -1589,6 +1595,18 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
/* set main state according to RC switches */
transition_result_t res = TRANSITION_DENIED;
/* offboard switch overrides main switch */
if (sp_man->offboard_switch == SWITCH_POS_ON) {
res = main_state_transition(status, MAIN_STATE_OFFBOARD);
if (res == TRANSITION_DENIED) {
print_reject_mode(status, "OFFBOARD");
} else {
return res;
}
}
/* offboard switched off or denied, check main mode switch */
switch (sp_man->mode_switch) {
case SWITCH_POS_NONE:
res = TRANSITION_NOT_CHANGED;
@ -1680,6 +1698,7 @@ set_control_mode()
/* 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_offboard_enabled = false;
switch (status.nav_state) {
case NAVIGATION_STATE_MANUAL:
@ -1718,6 +1737,54 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_OFFBOARD:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_offboard_enabled = true;
switch (sp_offboard.mode) {
case OFFBOARD_CONTROL_MODE_DIRECT_RATES:
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;
case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
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 OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY:
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = true;
break;
case OFFBOARD_CONTROL_MODE_DIRECT_POSITION:
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;
default:
control_mode.flag_control_rates_enabled = false;
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;
case NAVIGATION_STATE_POSCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;

1
src/modules/commander/px4_custom_mode.h
View File

@ -16,6 +16,7 @@ enum PX4_CUSTOM_MAIN_MODE {
PX4_CUSTOM_MAIN_MODE_POSCTL,
PX4_CUSTOM_MAIN_MODE_AUTO,
PX4_CUSTOM_MAIN_MODE_ACRO,
PX4_CUSTOM_MAIN_MODE_OFFBOARD,
};
enum PX4_CUSTOM_SUB_MODE_AUTO {

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

@ -234,6 +234,15 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
}
break;
case MAIN_STATE_OFFBOARD:
/* need offboard signal */
if (!status->offboard_control_signal_lost) {
ret = TRANSITION_CHANGED;
}
break;
case MAIN_STATE_MAX:
default:
break;

125
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -102,18 +102,25 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_cmd_pub(-1),
_flow_pub(-1),
_offboard_control_sp_pub(-1),
_local_pos_sp_pub(-1),
_global_vel_sp_pub(-1),
_att_sp_pub(-1),
_rates_sp_pub(-1),
_vicon_position_pub(-1),
_telemetry_status_pub(-1),
_rc_pub(-1),
_manual_pub(-1),
_telemetry_heartbeat_time(0),
_radio_status_available(false),
_control_mode_sub(-1),
_hil_frames(0),
_old_timestamp(0),
_hil_local_proj_inited(0),
_hil_local_alt0(0.0)
{
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
memset(&hil_local_pos, 0, sizeof(hil_local_pos));
memset(&_control_mode, 0, sizeof(_control_mode));
}
MavlinkReceiver::~MavlinkReceiver()
@ -355,12 +362,11 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
struct offboard_control_setpoint_s offboard_control_sp;
memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));
uint8_t ml_mode = 0;
enum OFFBOARD_CONTROL_MODE ml_mode = OFFBOARD_CONTROL_MODE_DIRECT;
bool ml_armed = false;
switch (quad_motors_setpoint.mode) {
case 0:
ml_armed = false;
break;
case 1:
@ -377,24 +383,43 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
case 3:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
ml_armed = true;
break;
case 4:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION;
ml_armed = true;
break;
default:
break;
}
offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX;
offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX;
offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX;
offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX;
if (ml_mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES || ml_mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX;
offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX;
offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX;
offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX;
} else if (ml_mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || ml_mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) {
/*Temporary hack to use set_quad_swarm_roll_pitch_yaw_thrust msg for position or velocity */
/* Converts INT16 centimeters to float meters */
offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / 100.0f;
offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / 100.0f;
offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / 100.0f;
offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / 100.0f;
}
if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) {
ml_armed = false;
}
offboard_control_sp.armed = ml_armed;
offboard_control_sp.mode = static_cast<enum OFFBOARD_CONTROL_MODE>(ml_mode);
offboard_control_sp.mode = ml_mode;
offboard_control_sp.timestamp = hrt_absolute_time();
@ -404,6 +429,90 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
} else {
orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
}
bool updated;
orb_check(_control_mode_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
if (_control_mode.flag_control_offboard_enabled) {
if (_control_mode.flag_control_position_enabled) {
// TODO Use something else then quad_swarm_roll_pitch_yaw_thrust
struct vehicle_local_position_setpoint_s loc_pos_sp;
memset(&loc_pos_sp, 0, sizeof(loc_pos_sp));
loc_pos_sp.x = offboard_control_sp.p1;
loc_pos_sp.y = offboard_control_sp.p2;
loc_pos_sp.yaw = offboard_control_sp.p3;
loc_pos_sp.z = -offboard_control_sp.p4;
if (_local_pos_sp_pub < 0) {
_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp_pub);
} else {
orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &loc_pos_sp);
}
} else if (_control_mode.flag_control_velocity_enabled) {
/* velocity control */
struct vehicle_global_velocity_setpoint_s global_vel_sp;
memset(&global_vel_sp, 0, sizeof(&global_vel_sp));
global_vel_sp.vx = offboard_control_sp.p1;
global_vel_sp.vy = offboard_control_sp.p2;
global_vel_sp.vz = offboard_control_sp.p3;
if (_global_vel_sp_pub < 0) {
_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp_pub);
} else {
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &global_vel_sp);
}
} else if (_control_mode.flag_control_attitude_enabled) {
/* attitude control */
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
att_sp.roll_body = offboard_control_sp.p1;
att_sp.pitch_body = offboard_control_sp.p2;
att_sp.yaw_body = offboard_control_sp.p3;
att_sp.thrust = offboard_control_sp.p4;
att_sp.timestamp = hrt_absolute_time();
if (_att_sp_pub < 0) {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
} else {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp);
}
} else if (_control_mode.flag_control_rates_enabled) {
/* rates control */
struct vehicle_rates_setpoint_s rates_sp;
memset(&rates_sp, 0, sizeof(rates_sp));
rates_sp.roll = offboard_control_sp.p1;
rates_sp.pitch = offboard_control_sp.p2;
rates_sp.yaw = offboard_control_sp.p3;
rates_sp.thrust = offboard_control_sp.p4;
rates_sp.timestamp = hrt_absolute_time();
if (_rates_sp_pub < 0) {
_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
} else {
orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp);
}
} else {
/* direct control */
// TODO
}
}
}
}
}
@ -453,6 +562,8 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
manual.r = man.r / 1000.0f;
manual.z = man.z / 1000.0f;
warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", manual.x, manual.y, manual.r, manual.z);
if (_manual_pub < 0) {
_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);

9
src/modules/mavlink/mavlink_receiver.h
View File

@ -36,6 +36,7 @@
* MAVLink 1.0 uORB listener definition
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#pragma once
@ -44,6 +45,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_position.h>
@ -53,6 +55,7 @@
#include <uORB/topics/offboard_control_setpoint.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
@ -121,6 +124,7 @@ private:
mavlink_status_t status;
struct vehicle_local_position_s hil_local_pos;
struct vehicle_control_mode_s _control_mode;
orb_advert_t _global_pos_pub;
orb_advert_t _local_pos_pub;
orb_advert_t _attitude_pub;
@ -135,12 +139,17 @@ private:
orb_advert_t _cmd_pub;
orb_advert_t _flow_pub;
orb_advert_t _offboard_control_sp_pub;
orb_advert_t _local_pos_sp_pub;
orb_advert_t _global_vel_sp_pub;
orb_advert_t _att_sp_pub;
orb_advert_t _rates_sp_pub;
orb_advert_t _vicon_position_pub;
orb_advert_t _telemetry_status_pub;
orb_advert_t _rc_pub;
orb_advert_t _manual_pub;
hrt_abstime _telemetry_heartbeat_time;
bool _radio_status_available;
int _control_mode_sub;
int _hil_frames;
uint64_t _old_timestamp;
bool _hil_local_proj_inited;

59
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -117,7 +117,9 @@ private:
int _manual_sub; /**< notification of manual control updates */
int _arming_sub; /**< arming status of outputs */
int _local_pos_sub; /**< vehicle local position */
int _pos_sp_triplet_sub; /**< position setpoint triplet */
int _pos_sp_triplet_sub; /**< position setpoint triplet */
int _local_pos_sp_sub; /**< offboard local position setpoint */
int _global_vel_sp_sub; /**< offboard global velocity setpoint */
orb_advert_t _att_sp_pub; /**< attitude setpoint publication */
orb_advert_t _local_pos_sp_pub; /**< vehicle local position setpoint publication */
@ -132,6 +134,7 @@ private:
struct position_setpoint_triplet_s _pos_sp_triplet; /**< vehicle global position setpoint triplet */
struct vehicle_local_position_setpoint_s _local_pos_sp; /**< vehicle local position setpoint */
struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */
struct {
param_t thr_min;
@ -255,6 +258,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_manual_sub(-1),
_arming_sub(-1),
_local_pos_sub(-1),
_global_vel_sp_sub(-1),
_pos_sp_triplet_sub(-1),
/* publications */
@ -528,6 +532,7 @@ MulticopterPositionControl::task_main()
_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
parameters_update(true);
@ -664,6 +669,37 @@ MulticopterPositionControl::task_main()
_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
}
} else if (_control_mode.flag_control_offboard_enabled) {
/* Offboard control */
bool updated;
orb_check(_local_pos_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_sub, &_local_pos_sp);
}
if (isfinite(_local_pos_sp.x) && isfinite(_local_pos_sp.y) && isfinite(_local_pos_sp.z) && isfinite(_local_pos_sp.yaw)) {
/* If manual control overides offboard, cancel the offboard setpoint and stay at current position */
_reset_pos_sp = true;
_reset_alt_sp = true;
/* Make sure position control is selected i.e. not only velocity control */
if (_control_mode.flag_control_position_enabled) {
_pos_sp(0) = _local_pos_sp.x;
_pos_sp(1) = _local_pos_sp.y;
}
if (_control_mode.flag_control_altitude_enabled) {
_pos_sp(2) = _local_pos_sp.z;
}
_att_sp.yaw_body = _local_pos_sp.yaw;
} else {
reset_pos_sp();
reset_alt_sp();
}
} else {
/* AUTO */
bool updated;
@ -710,6 +746,7 @@ MulticopterPositionControl::task_main()
_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);
}
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) {
/* idle state, don't run controller and set zero thrust */
R.identity();
@ -753,6 +790,25 @@ MulticopterPositionControl::task_main()
_vel_sp(2) = _params.land_speed;
}
/* Offboard velocity control mode */
if (_control_mode.flag_control_offboard_enabled) {
bool updated;
orb_check(_global_vel_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_sub, &_global_vel_sp);
}
if (!_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_climb_rate_enabled) {
_vel_sp(2) = _global_vel_sp.vz;
}
if (!_control_mode.flag_control_position_enabled && _control_mode.flag_control_velocity_enabled) {
_vel_sp(0) = _global_vel_sp.vx;
_vel_sp(1) = _global_vel_sp.vy;
}
}
if (!_control_mode.flag_control_manual_enabled) {
/* limit 3D speed only in non-manual modes */
float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length();
@ -1040,6 +1096,7 @@ MulticopterPositionControl::task_main()
_reset_pos_sp = true;
reset_int_z = true;
reset_int_xy = true;
}
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */

9
src/modules/sensors/sensor_params.c
View File

@ -634,6 +634,15 @@ PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0);
*/
PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0);
/**
* Offboard switch channel mapping.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
/**
* Flaps channel mapping.
*

16
src/modules/sensors/sensors.cpp
View File

@ -264,6 +264,7 @@ private:
int rc_map_posctl_sw;
int rc_map_loiter_sw;
int rc_map_acro_sw;
int rc_map_offboard_sw;
int rc_map_flaps;
@ -280,12 +281,14 @@ private:
float rc_return_th;
float rc_loiter_th;
float rc_acro_th;
float rc_offboard_th;
bool rc_assist_inv;
bool rc_auto_inv;
bool rc_posctl_inv;
bool rc_return_inv;
bool rc_loiter_inv;
bool rc_acro_inv;
bool rc_offboard_inv;
float battery_voltage_scaling;
float battery_current_scaling;
@ -319,6 +322,7 @@ private:
param_t rc_map_posctl_sw;
param_t rc_map_loiter_sw;
param_t rc_map_acro_sw;
param_t rc_map_offboard_sw;
param_t rc_map_flaps;
@ -335,6 +339,7 @@ private:
param_t rc_return_th;
param_t rc_loiter_th;
param_t rc_acro_th;
param_t rc_offboard_th;
param_t battery_voltage_scaling;
param_t battery_current_scaling;
@ -536,6 +541,7 @@ Sensors::Sensors() :
_parameter_handles.rc_map_posctl_sw = param_find("RC_MAP_POSCTL_SW");
_parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW");
_parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW");
_parameter_handles.rc_map_offboard_sw = param_find("RC_MAP_OFFB_SW");
_parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1");
_parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2");
@ -551,6 +557,7 @@ Sensors::Sensors() :
_parameter_handles.rc_return_th = param_find("RC_RETURN_TH");
_parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH");
_parameter_handles.rc_acro_th = param_find("RC_ACRO_TH");
_parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH");
/* gyro offsets */
_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
@ -698,6 +705,10 @@ Sensors::parameters_update()
warnx("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_offboard_sw, &(_parameters.rc_map_offboard_sw)) != OK) {
warnx("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
warnx("%s", paramerr);
}
@ -726,6 +737,9 @@ Sensors::parameters_update()
param_get(_parameter_handles.rc_acro_th, &(_parameters.rc_acro_th));
_parameters.rc_acro_inv = (_parameters.rc_acro_th < 0);
_parameters.rc_acro_th = fabs(_parameters.rc_acro_th);
param_get(_parameter_handles.rc_offboard_th, &(_parameters.rc_offboard_th));
_parameters.rc_offboard_inv = (_parameters.rc_offboard_th < 0);
_parameters.rc_offboard_th = fabs(_parameters.rc_offboard_th);
/* update RC function mappings */
_rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
@ -738,6 +752,7 @@ Sensors::parameters_update()
_rc.function[POSCTL] = _parameters.rc_map_posctl_sw - 1;
_rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1;
_rc.function[ACRO] = _parameters.rc_map_acro_sw - 1;
_rc.function[OFFBOARD] = _parameters.rc_map_offboard_sw - 1;
_rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
@ -1524,6 +1539,7 @@ Sensors::rc_poll()
manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv);
manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv);
manual.acro_switch = get_rc_sw2pos_position(ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv);
manual.offboard_switch = get_rc_sw2pos_position(OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv);
/* publish manual_control_setpoint topic */
if (_manual_control_pub > 0) {

1
src/modules/uORB/topics/manual_control_setpoint.h
View File

@ -98,6 +98,7 @@ struct manual_control_setpoint_s {
switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */
switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */
switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */
switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */
}; /**< manual control inputs */
/**

2
src/modules/uORB/topics/rc_channels.h
View File

@ -66,7 +66,7 @@ enum RC_CHANNELS_FUNCTION {
RETURN = 5,
POSCTL = 6,
LOITER = 7,
OFFBOARD_MODE = 8,
OFFBOARD = 8,
ACRO = 9,
FLAPS = 10,
AUX_1 = 11,

1
src/modules/uORB/topics/vehicle_control_mode.h
View File

@ -74,6 +74,7 @@ struct vehicle_control_mode_s {
bool flag_control_manual_enabled; /**< true if manual input is mixed in */
bool flag_control_auto_enabled; /**< true if onboard autopilot should act */
bool flag_control_offboard_enabled; /**< true if offboard control should be used */
bool flag_control_rates_enabled; /**< true if rates are stabilized */
bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */
bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */

4
src/modules/uORB/topics/vehicle_status.h
View File

@ -70,7 +70,8 @@ typedef enum {
MAIN_STATE_AUTO_LOITER,
MAIN_STATE_AUTO_RTL,
MAIN_STATE_ACRO,
MAIN_STATE_MAX,
MAIN_STATE_OFFBOARD,
MAIN_STATE_MAX
} main_state_t;
// If you change the order, add or remove arming_state_t states make sure to update the arrays
@ -106,6 +107,7 @@ typedef enum {
NAVIGATION_STATE_LAND, /**< Land mode */
NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */
NAVIGATION_STATE_TERMINATION, /**< Termination mode */
NAVIGATION_STATE_OFFBOARD,
NAVIGATION_STATE_MAX,
} navigation_state_t;