Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 04:33:10 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

Merge branch 'rc_timeout' of https://github.com/TickTock-/Firmware into rc_merged

Browse Source
This commit is contained in:
TickTock-
2014-04-22 17:43:36 -07:00
parent 86a0862af6 0b97dd2b77
commit d6e6ee3440
16 changed files with 340 additions and 468 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+66 -73
View File
@@ -67,6 +67,7 @@
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/subsystem_info.h>
@@ -112,8 +113,7 @@ extern struct system_load_s system_load;
#define MAVLINK_OPEN_INTERVAL 50000
#define STICK_ON_OFF_LIMIT 0.75f
#define STICK_THRUST_RANGE 1.0f
#define STICK_ON_OFF_LIMIT 0.9f
#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
@@ -207,7 +207,7 @@ void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool
void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status);
transition_result_t set_main_state_rc(struct vehicle_status_s *status);
transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man);
void set_control_mode();
@@ -819,6 +819,11 @@ int commander_thread_main(int argc, char *argv[])
struct subsystem_info_s info;
memset(&info, 0, sizeof(info));
/* Subscribe to position setpoint triplet */
int pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
struct position_setpoint_triplet_s pos_sp_triplet;
memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
control_status_leds(&status, &armed, true);
/* now initialized */
@@ -1010,6 +1015,13 @@ int commander_thread_main(int argc, char *argv[])
status_changed = true;
}
/* update subsystem */
orb_check(pos_sp_triplet_sub, &updated);
if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet);
}
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
/* compute system load */
uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;
@@ -1140,7 +1152,7 @@ int commander_thread_main(int argc, char *argv[])
if (status.is_rotary_wing &&
(status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
(status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) {
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
@@ -1158,7 +1170,7 @@ int commander_thread_main(int argc, char *argv[])
/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
if (status.arming_state == ARMING_STATE_STANDBY &&
sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) {
print_reject_arm("NOT ARMING: Press safety switch first.");
@@ -1198,11 +1210,8 @@ int commander_thread_main(int argc, char *argv[])
transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
}
/* fill status according to mode switches */
check_mode_switches(&sp_man, &status);
/* evaluate the main state machine according to mode switches */
res = set_main_state_rc(&status);
res = set_main_state_rc(&status, &sp_man);
/* play tune on mode change only if armed, blink LED always */
if (res == TRANSITION_CHANGED) {
@@ -1213,6 +1222,33 @@ int commander_thread_main(int argc, char *argv[])
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 {
/* MISSION switch */
if (sp_man.mission_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.mission_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");
@@ -1260,7 +1296,7 @@ int commander_thread_main(int argc, char *argv[])
// TODO remove this hack
/* flight termination in manual mode if assisted switch is on easy position */
if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch == SWITCH_POS_ON) {
if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
tune_positive(armed.armed);
}
@@ -1475,76 +1511,29 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
leds_counter++;
}
void
check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status)
{
/* main mode switch */
if (!isfinite(sp_man->mode_switch)) {
/* default to manual if signal is invalid */
status->mode_switch = MODE_SWITCH_MANUAL;
} else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) {
status->mode_switch = MODE_SWITCH_AUTO;
} else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) {
status->mode_switch = MODE_SWITCH_MANUAL;
} else {
status->mode_switch = MODE_SWITCH_ASSISTED;
}
/* return switch */
if (!isfinite(sp_man->return_switch)) {
status->return_switch = RETURN_SWITCH_NONE;
} else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) {
status->return_switch = RETURN_SWITCH_RETURN;
} else {
status->return_switch = RETURN_SWITCH_NORMAL;
}
/* assisted switch */
if (!isfinite(sp_man->assisted_switch)) {
status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
} else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) {
status->assisted_switch = ASSISTED_SWITCH_EASY;
} else {
status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
}
/* mission switch */
if (!isfinite(sp_man->mission_switch)) {
status->mission_switch = MISSION_SWITCH_NONE;
} else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) {
status->mission_switch = MISSION_SWITCH_LOITER;
} else {
status->mission_switch = MISSION_SWITCH_MISSION;
}
}
transition_result_t
set_main_state_rc(struct vehicle_status_s *status)
set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man)
{
/* set main state according to RC switches */
transition_result_t res = TRANSITION_DENIED;
switch (status->mode_switch) {
case MODE_SWITCH_MANUAL:
switch (sp_man->mode_switch) {
case SWITCH_POS_NONE:
res = TRANSITION_NOT_CHANGED;
break;
case SWITCH_POS_OFF: // MANUAL
res = main_state_transition(status, MAIN_STATE_MANUAL);
// TRANSITION_DENIED is not possible here
break;
case MODE_SWITCH_ASSISTED:
if (status->assisted_switch == ASSISTED_SWITCH_EASY) {
case SWITCH_POS_MIDDLE: // ASSISTED
if (sp_man->assisted_switch == SWITCH_POS_ON) {
res = main_state_transition(status, MAIN_STATE_EASY);
if (res != TRANSITION_DENIED)
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// else fallback to SEATBELT
print_reject_mode(status, "EASY");
@@ -1552,29 +1541,33 @@ set_main_state_rc(struct vehicle_status_s *status)
res = main_state_transition(status, MAIN_STATE_SEATBELT);
if (res != TRANSITION_DENIED)
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this mode
}
if (status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages
if (sp_man->assisted_switch != SWITCH_POS_ON) {
print_reject_mode(status, "SEATBELT");
}
// else fallback to MANUAL
res = main_state_transition(status, MAIN_STATE_MANUAL);
// TRANSITION_DENIED is not possible here
break;
case MODE_SWITCH_AUTO:
case SWITCH_POS_ON: // AUTO
res = main_state_transition(status, MAIN_STATE_AUTO);
if (res != TRANSITION_DENIED)
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// else fallback to SEATBELT (EASY likely will not work too)
print_reject_mode(status, "AUTO");
res = main_state_transition(status, MAIN_STATE_SEATBELT);
if (res != TRANSITION_DENIED)
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// else fallback to MANUAL
res = main_state_transition(status, MAIN_STATE_MANUAL);
src/modules/fw_att_control/fw_att_control_main.cpp
+14 -3
View File
@@ -173,6 +173,8 @@ private:
float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */
float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */
float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */
float man_roll_max; /**< Max Roll in rad */
float man_pitch_max; /**< Max Pitch in rad */
} _parameters; /**< local copies of interesting parameters */
@@ -211,6 +213,8 @@ private:
param_t trim_yaw;
param_t rollsp_offset_deg;
param_t pitchsp_offset_deg;
param_t man_roll_max;
param_t man_pitch_max;
} _parameter_handles; /**< handles for interesting parameters */
@@ -354,6 +358,9 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF");
_parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF");
_parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX");
_parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX");
/* fetch initial parameter values */
parameters_update();
}
@@ -421,6 +428,10 @@ FixedwingAttitudeControl::parameters_update()
param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg));
_parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg);
_parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg);
param_get(_parameter_handles.man_roll_max, &(_parameters.man_roll_max));
param_get(_parameter_handles.man_pitch_max, &(_parameters.man_pitch_max));
_parameters.man_roll_max = math::radians(_parameters.man_roll_max);
_parameters.man_pitch_max = math::radians(_parameters.man_pitch_max);
/* pitch control parameters */
@@ -706,8 +717,8 @@ FixedwingAttitudeControl::task_main()
* the intended attitude setpoint. Later, after the rate control step the
* trim is added again to the control signal.
*/
roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f + _parameters.rollsp_offset_rad;
pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f + _parameters.pitchsp_offset_rad;
roll_sp = (_manual.roll * _parameters.man_roll_max - _parameters.trim_roll) + _parameters.rollsp_offset_rad;
pitch_sp = (-_manual.pitch * _parameters.man_pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad;
throttle_sp = _manual.throttle;
_actuators.control[4] = _manual.flaps;
@@ -815,7 +826,7 @@ FixedwingAttitudeControl::task_main()
} else {
/* manual/direct control */
_actuators.control[0] = _manual.roll;
_actuators.control[1] = _manual.pitch;
_actuators.control[1] = -_manual.pitch;
_actuators.control[2] = _manual.yaw;
_actuators.control[3] = _manual.throttle;
_actuators.control[4] = _manual.flaps;
src/modules/fw_att_control/fw_att_control_params.c
+10
View File
@@ -186,3 +186,13 @@ PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f);
// @Description An airframe specific offset of the pitch setpoint in degrees, the value is added to the pitch setpoint and should correspond to the typical cruise speed of the airframe
// @Range -90.0 to 90.0
PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f);
// @DisplayName Max Manual Roll
// @Description Max roll for manual control in attitude stabilized mode
// @Range 0.0 to 90.0
PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f);
// @DisplayName Max Manual Pitch
// @Description Max pitch for manual control in attitude stabilized mode
// @Range 0.0 to 90.0
PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f);
src/modules/mavlink/mavlink_receiver.cpp
+11 -42
View File
@@ -88,8 +88,6 @@ static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f;
MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_mavlink(parent),
_manual_sub(-1),
_global_pos_pub(-1),
_local_pos_pub(-1),
_attitude_pub(-1),
@@ -417,47 +415,20 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
mavlink_manual_control_t man;
mavlink_msg_manual_control_decode(msg, &man);
/* rc channels */
{
struct rc_channels_s rc;
memset(&rc, 0, sizeof(rc));
struct manual_control_setpoint_s manual;
memset(&manual, 0, sizeof(manual));
rc.timestamp = hrt_absolute_time();
rc.chan_count = 4;
manual.timestamp = hrt_absolute_time();
manual.roll = man.x / 1000.0f;
manual.pitch = man.y / 1000.0f;
manual.yaw = man.r / 1000.0f;
manual.throttle = man.z / 1000.0f;
rc.chan[0].scaled = man.x / 1000.0f;
rc.chan[1].scaled = man.y / 1000.0f;
rc.chan[2].scaled = man.r / 1000.0f;
rc.chan[3].scaled = man.z / 1000.0f;
if (_manual_pub < 0) {
_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
if (_rc_pub < 0) {
_rc_pub = orb_advertise(ORB_ID(rc_channels), &rc);
} else {
orb_publish(ORB_ID(rc_channels), _rc_pub, &rc);
}
}
/* manual control */
{
struct manual_control_setpoint_s manual;
memset(&manual, 0, sizeof(manual));
/* get a copy first, to prevent altering values that are not sent by the mavlink command */
orb_copy(ORB_ID(manual_control_setpoint), _manual_sub, &manual);
manual.timestamp = hrt_absolute_time();
manual.roll = man.x / 1000.0f;
manual.pitch = man.y / 1000.0f;
manual.yaw = man.r / 1000.0f;
manual.throttle = man.z / 1000.0f;
if (_manual_pub < 0) {
_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
} else {
orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual);
}
} else {
orb_publish(ORB_ID(manual_control_setpoint), _manual_pub, &manual);
}
}
@@ -887,8 +858,6 @@ MavlinkReceiver::receive_thread(void *arg)
sprintf(thread_name, "mavlink_rcv_if%d", _mavlink->get_instance_id());
prctl(PR_SET_NAME, thread_name, getpid());
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
struct pollfd fds[1];
fds[0].fd = uart_fd;
fds[0].events = POLLIN;
src/modules/mavlink/mavlink_receiver.h
-1
View File
@@ -138,7 +138,6 @@ private:
orb_advert_t _telemetry_status_pub;
orb_advert_t _rc_pub;
orb_advert_t _manual_pub;
int _manual_sub;
int _hil_frames;
uint64_t _old_timestamp;
bool _hil_local_proj_inited;
src/modules/mc_att_control/mc_att_control_main.cpp
+26 -28
View File
@@ -157,7 +157,9 @@ private:
param_t yaw_rate_d;
param_t yaw_ff;
param_t rc_scale_yaw;
param_t man_roll_max;
param_t man_pitch_max;
param_t man_yaw_max;
} _params_handles; /**< handles for interesting parameters */
struct {
@@ -167,7 +169,9 @@ private:
math::Vector<3> rate_d; /**< D gain for angular rate error */
float yaw_ff; /**< yaw control feed-forward */
float rc_scale_yaw;
float man_roll_max;
float man_pitch_max;
float man_yaw_max;
} _params;
/**
@@ -295,7 +299,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params_handles.yaw_rate_d = param_find("MC_YAWRATE_D");
_params_handles.yaw_ff = param_find("MC_YAW_FF");
_params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW");
_params_handles.man_roll_max = param_find("MC_MAN_R_MAX");
_params_handles.man_pitch_max = param_find("MC_MAN_P_MAX");
_params_handles.man_yaw_max = param_find("MC_MAN_Y_MAX");
/* fetch initial parameter values */
parameters_update();
@@ -330,7 +336,7 @@ MulticopterAttitudeControl::parameters_update()
{
float v;
/* roll */
/* roll gains */
param_get(_params_handles.roll_p, &v);
_params.att_p(0) = v;
param_get(_params_handles.roll_rate_p, &v);
@@ -340,7 +346,7 @@ MulticopterAttitudeControl::parameters_update()
param_get(_params_handles.roll_rate_d, &v);
_params.rate_d(0) = v;
/* pitch */
/* pitch gains */
param_get(_params_handles.pitch_p, &v);
_params.att_p(1) = v;
param_get(_params_handles.pitch_rate_p, &v);
@@ -350,7 +356,7 @@ MulticopterAttitudeControl::parameters_update()
param_get(_params_handles.pitch_rate_d, &v);
_params.rate_d(1) = v;
/* yaw */
/* yaw gains */
param_get(_params_handles.yaw_p, &v);
_params.att_p(2) = v;
param_get(_params_handles.yaw_rate_p, &v);
@@ -362,7 +368,14 @@ MulticopterAttitudeControl::parameters_update()
param_get(_params_handles.yaw_ff, &_params.yaw_ff);
param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw);
/* manual control scale */
param_get(_params_handles.man_roll_max, &_params.man_roll_max);
param_get(_params_handles.man_pitch_max, &_params.man_pitch_max);
param_get(_params_handles.man_yaw_max, &_params.man_yaw_max);
_params.man_roll_max *= M_PI / 180.0;
_params.man_pitch_max *= M_PI / 180.0;
_params.man_yaw_max *= M_PI / 180.0;
return OK;
}
@@ -404,7 +417,6 @@ MulticopterAttitudeControl::vehicle_manual_poll()
orb_check(_manual_control_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
}
}
@@ -483,24 +495,10 @@ MulticopterAttitudeControl::control_attitude(float dt)
// reset_yaw_sp = true;
//}
} else {
float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw;
if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) {
/* move yaw setpoint */
yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw;
if (_manual_control_sp.yaw > 0.0f) {
yaw_sp_move_rate -= YAW_DEADZONE;
} else {
yaw_sp_move_rate += YAW_DEADZONE;
}
yaw_sp_move_rate *= _params.rc_scale_yaw;
_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
_v_att_sp.R_valid = false;
publish_att_sp = true;
}
/* move yaw setpoint */
_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + _manual_control_sp.yaw * _params.man_yaw_max * dt);
_v_att_sp.R_valid = false;
publish_att_sp = true;
}
/* reset yaw setpint to current position if needed */
@@ -513,8 +511,8 @@ MulticopterAttitudeControl::control_attitude(float dt)
if (!_v_control_mode.flag_control_velocity_enabled) {
/* update attitude setpoint if not in position control mode */
_v_att_sp.roll_body = _manual_control_sp.roll;
_v_att_sp.pitch_body = _manual_control_sp.pitch;
_v_att_sp.roll_body = _manual_control_sp.roll * _params.man_roll_max;
_v_att_sp.pitch_body = -_manual_control_sp.pitch * _params.man_pitch_max;
_v_att_sp.R_valid = false;
publish_att_sp = true;
}
src/modules/mc_att_control/mc_att_control_params.c
+29
View File
@@ -173,3 +173,32 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f);
/**
* Max manual roll
*
* @unit deg
* @min 0.0
* @max 90.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_MAN_R_MAX, 35.0f);
/**
* Max manual pitch
*
* @unit deg
* @min 0.0
* @max 90.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_MAN_P_MAX, 35.0f);
/**
* Max manual yaw rate
*
* @unit deg/s
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX, 120.0f);
src/modules/mc_pos_control/mc_pos_control_main.cpp
+2 -12
View File
@@ -148,9 +148,6 @@ private:
param_t tilt_max;
param_t land_speed;
param_t land_tilt_max;
param_t rc_scale_pitch;
param_t rc_scale_roll;
} _params_handles; /**< handles for interesting parameters */
struct {
@@ -160,9 +157,6 @@ private:
float land_speed;
float land_tilt_max;
float rc_scale_pitch;
float rc_scale_roll;
math::Vector<3> pos_p;
math::Vector<3> vel_p;
math::Vector<3> vel_i;
@@ -306,8 +300,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
_params_handles.tilt_max = param_find("MPC_TILT_MAX");
_params_handles.land_speed = param_find("MPC_LAND_SPEED");
_params_handles.land_tilt_max = param_find("MPC_LAND_TILT");
_params_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH");
_params_handles.rc_scale_roll = param_find("RC_SCALE_ROLL");
/* fetch initial parameter values */
parameters_update(true);
@@ -354,8 +346,6 @@ MulticopterPositionControl::parameters_update(bool force)
param_get(_params_handles.tilt_max, &_params.tilt_max);
param_get(_params_handles.land_speed, &_params.land_speed);
param_get(_params_handles.land_tilt_max, &_params.land_tilt_max);
param_get(_params_handles.rc_scale_pitch, &_params.rc_scale_pitch);
param_get(_params_handles.rc_scale_roll, &_params.rc_scale_roll);
float v;
param_get(_params_handles.xy_p, &v);
@@ -608,8 +598,8 @@ MulticopterPositionControl::task_main()
reset_lat_lon_sp();
/* move position setpoint with roll/pitch stick */
sp_move_rate(0) = scale_control(-_manual.pitch / _params.rc_scale_pitch, 1.0f, pos_ctl_dz);
sp_move_rate(1) = scale_control(_manual.roll / _params.rc_scale_roll, 1.0f, pos_ctl_dz);
sp_move_rate(0) = _manual.pitch;
sp_move_rate(1) = _manual.roll;
}
/* limit setpoint move rate */
src/modules/navigator/navigator_main.cpp
+31 -67
View File
@@ -690,84 +690,46 @@ Navigator::task_main()
if (fds[6].revents & POLLIN) {
vehicle_status_update();
/* evaluate state machine from commander and set the navigator mode accordingly */
/* evaluate state requested by commander */
if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
bool stick_mode = false;
if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
/* commander requested new navigation mode, try to set it */
switch (_vstatus.set_nav_state) {
case NAV_STATE_NONE:
/* nothing to do */
break;
if (!_vstatus.rc_signal_lost) {
/* RC signal available, use control switches to set mode */
/* RETURN switch, overrides MISSION switch */
if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
/* switch to RTL if not already landed after RTL and home position set */
case NAV_STATE_LOITER:
request_loiter_or_ready();
break;
case NAV_STATE_MISSION:
request_mission_if_available();
break;
case NAV_STATE_RTL:
if (!(_rtl_state == RTL_STATE_DESCEND &&
(myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
_vstatus.condition_home_position_valid) {
(myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
_vstatus.condition_home_position_valid) {
dispatch(EVENT_RTL_REQUESTED);
}
stick_mode = true;
break;
} else {
/* MISSION switch */
if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
request_loiter_or_ready();
stick_mode = true;
case NAV_STATE_LAND:
dispatch(EVENT_LAND_REQUESTED);
} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
request_mission_if_available();
stick_mode = true;
}
break;
if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
request_mission_if_available();
stick_mode = true;
}
default:
warnx("ERROR: Requested navigation state not supported");
break;
}
}
if (!stick_mode) {
if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
/* commander requested new navigation mode, try to set it */
_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
switch (_vstatus.set_nav_state) {
case NAV_STATE_NONE:
/* nothing to do */
break;
case NAV_STATE_LOITER:
request_loiter_or_ready();
break;
case NAV_STATE_MISSION:
request_mission_if_available();
break;
case NAV_STATE_RTL:
if (!(_rtl_state == RTL_STATE_DESCEND &&
(myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
_vstatus.condition_home_position_valid) {
dispatch(EVENT_RTL_REQUESTED);
}
break;
case NAV_STATE_LAND:
dispatch(EVENT_LAND_REQUESTED);
break;
default:
warnx("ERROR: Requested navigation state not supported");
break;
}
} else {
/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
if (myState == NAV_STATE_NONE) {
request_mission_if_available();
}
} else {
/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
if (myState == NAV_STATE_NONE) {
request_mission_if_available();
}
}
@@ -775,6 +737,8 @@ Navigator::task_main()
/* navigator shouldn't act */
dispatch(EVENT_NONE_REQUESTED);
}
_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
}
/* parameters updated */
src/modules/sdlog2/sdlog2.c
+1
View File
@@ -1193,6 +1193,7 @@ int sdlog2_thread_main(int argc, char *argv[])
/* Copy only the first 8 channels of 14 */
memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel));
log_msg.body.log_RC.channel_count = buf.rc.chan_count;
log_msg.body.log_RC.signal_lost = buf.rc.signal_lost;
LOGBUFFER_WRITE_AND_COUNT(RC);
}
src/modules/sdlog2/sdlog2_messages.h
+2 -1
View File
@@ -161,6 +161,7 @@ struct log_STAT_s {
struct log_RC_s {
float channel[8];
uint8_t channel_count;
uint8_t signal_lost;
};
/* --- OUT0 - ACTUATOR_0 OUTPUT --- */
@@ -348,7 +349,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"),
LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"),
LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
src/modules/sensors/sensor_params.c
-21
View File
@@ -676,27 +676,6 @@ PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera roll */
PARAM_DEFINE_INT32(RC_MAP_AUX3, 0);
/**
* Roll scaling factor
*
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f);
/**
* Pitch scaling factor
*
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f);
/**
* Yaw scaling factor
*
* @group Radio Calibration
*/
PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f);
/**
* Failsafe channel PWM threshold.
*
src/modules/sensors/sensors.cpp
+127 -175
View File
@@ -135,7 +135,7 @@
*/
#define PCB_TEMP_ESTIMATE_DEG 5.0f
#define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
#define STICK_ON_OFF_LIMIT 0.75f
/**
* Sensor app start / stop handling function
@@ -167,7 +167,15 @@ public:
private:
static const unsigned _rc_max_chan_count = RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */
hrt_abstime _rc_last_valid; /**< last time we got a valid RC signal */
/**
* Get and limit value for specified RC function. Returns NAN if not mapped.
*/
float get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value);
/**
* Get switch position for specified function.
*/
switch_pos_t get_rc_switch_position(enum RC_CHANNELS_FUNCTION func);
/**
* Gather and publish RC input data.
@@ -242,7 +250,6 @@ private:
int rc_map_pitch;
int rc_map_yaw;
int rc_map_throttle;
int rc_map_failsafe;
int rc_map_mode_sw;
int rc_map_return_sw;
@@ -259,11 +266,6 @@ private:
int rc_map_aux4;
int rc_map_aux5;
float rc_scale_roll;
float rc_scale_pitch;
float rc_scale_yaw;
float rc_scale_flaps;
int32_t rc_fs_thr;
float battery_voltage_scaling;
@@ -291,7 +293,6 @@ private:
param_t rc_map_pitch;
param_t rc_map_yaw;
param_t rc_map_throttle;
param_t rc_map_failsafe;
param_t rc_map_mode_sw;
param_t rc_map_return_sw;
@@ -308,11 +309,6 @@ private:
param_t rc_map_aux4;
param_t rc_map_aux5;
param_t rc_scale_roll;
param_t rc_scale_pitch;
param_t rc_scale_yaw;
param_t rc_scale_flaps;
param_t rc_fs_thr;
param_t battery_voltage_scaling;
@@ -436,8 +432,6 @@ Sensors *g_sensors = nullptr;
}
Sensors::Sensors() :
_rc_last_valid(0),
_fd_adc(-1),
_last_adc(0),
@@ -472,6 +466,7 @@ Sensors::Sensors() :
_battery_discharged(0),
_battery_current_timestamp(0)
{
memset(&_rc, 0, sizeof(_rc));
/* basic r/c parameters */
for (unsigned i = 0; i < _rc_max_chan_count; i++) {
@@ -514,7 +509,6 @@ Sensors::Sensors() :
/* optional mode switches, not mapped per default */
_parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW");
_parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW");
_parameter_handles.rc_map_failsafe = param_find("RC_MAP_FAILSAFE");
// _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW");
@@ -524,11 +518,6 @@ Sensors::Sensors() :
_parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4");
_parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5");
_parameter_handles.rc_scale_roll = param_find("RC_SCALE_ROLL");
_parameter_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH");
_parameter_handles.rc_scale_yaw = param_find("RC_SCALE_YAW");
_parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS");
/* RC failsafe */
_parameter_handles.rc_fs_thr = param_find("RC_FAILS_THR");
@@ -653,10 +642,6 @@ Sensors::parameters_update()
warnx(paramerr);
}
if (param_get(_parameter_handles.rc_map_failsafe, &(_parameters.rc_map_failsafe)) != OK) {
warnx(paramerr);
}
if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) {
warnx(paramerr);
}
@@ -686,10 +671,6 @@ Sensors::parameters_update()
param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3));
param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4));
param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5));
param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll));
param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch));
param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw));
param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps));
param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr));
/* update RC function mappings */
@@ -711,6 +692,7 @@ Sensors::parameters_update()
_rc.function[AUX_2] = _parameters.rc_map_aux2 - 1;
_rc.function[AUX_3] = _parameters.rc_map_aux3 - 1;
_rc.function[AUX_4] = _parameters.rc_map_aux4 - 1;
_rc.function[AUX_5] = _parameters.rc_map_aux5 - 1;
/* gyro offsets */
param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0]));
@@ -1273,6 +1255,45 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
}
}
float
Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value)
{
if (_rc.function[func] >= 0) {
float value = _rc.chan[_rc.function[func]].scaled;
if (value < min_value) {
return min_value;
} else if (value > max_value) {
return max_value;
} else {
return value;
}
} else {
return 0.0f;
}
}
switch_pos_t
Sensors::get_rc_switch_position(enum RC_CHANNELS_FUNCTION func)
{
if (_rc.function[func] >= 0) {
float value = _rc.chan[_rc.function[func]].scaled;
if (value > STICK_ON_OFF_LIMIT) {
return SWITCH_POS_ON;
} else if (value < -STICK_ON_OFF_LIMIT) {
return SWITCH_POS_OFF;
} else {
return SWITCH_POS_MIDDLE;
}
} else {
return SWITCH_POS_NONE;
}
}
void
Sensors::rc_poll()
{
@@ -1281,45 +1302,32 @@ Sensors::rc_poll()
if (rc_updated) {
/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
struct rc_input_values rc_input;
struct rc_input_values rc_input;
orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
if (rc_input.rc_lost)
return;
/* detect RC signal loss */
bool signal_lost;
struct manual_control_setpoint_s manual_control;
struct actuator_controls_s actuator_group_3;
/* check flags and require at least four channels to consider the signal valid */
if (rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4) {
/* signal is lost or no enough channels */
signal_lost = true;
/* initialize to default values */
manual_control.roll = NAN;
manual_control.pitch = NAN;
manual_control.yaw = NAN;
manual_control.throttle = NAN;
} else {
/* signal looks good */
signal_lost = false;
manual_control.mode_switch = NAN;
manual_control.return_switch = NAN;
manual_control.assisted_switch = NAN;
manual_control.mission_switch = NAN;
// manual_control.auto_offboard_input_switch = NAN;
manual_control.flaps = NAN;
manual_control.aux1 = NAN;
manual_control.aux2 = NAN;
manual_control.aux3 = NAN;
manual_control.aux4 = NAN;
manual_control.aux5 = NAN;
/* require at least four channels to consider the signal valid */
if (rc_input.channel_count < 4) {
return;
}
/* check for failsafe */
if ((rc_input.rc_failsafe) || ((_parameters.rc_fs_thr != 0) && (((rc_input.values[_rc.function[_parameters.rc_map_failsafe]] < _parameters.min[_rc.function[_parameters.rc_map_failsafe]]) && (rc_input.values[_rc.function[_parameters.rc_map_failsafe]] < _parameters.rc_fs_thr))
|| ((rc_input.values[_rc.function[_parameters.rc_map_failsafe]] > _parameters.max[_rc.function[_parameters.rc_map_failsafe]]) && (rc_input.values[_rc.function[_parameters.rc_map_failsafe]] > _parameters.rc_fs_thr))))) {
/* do not publish manual control setpoints when there are none */
return;
/* check throttle failsafe */
int8_t thr_ch = _rc.function[THROTTLE];
if (_parameters.rc_fs_thr > 0 && thr_ch >= 0) {
/* throttle failsafe configured */
if ((_parameters.rc_fs_thr < _parameters.min[thr_ch] && rc_input.values[thr_ch] < _parameters.rc_fs_thr) ||
(_parameters.rc_fs_thr > _parameters.max[thr_ch] && rc_input.values[thr_ch] > _parameters.rc_fs_thr)) {
/* throttle failsafe triggered, signal is lost by receiver */
signal_lost = true;
}
}
}
unsigned channel_limit = rc_input.channel_count;
@@ -1327,10 +1335,7 @@ Sensors::rc_poll()
if (channel_limit > _rc_max_chan_count)
channel_limit = _rc_max_chan_count;
/* we are accepting this message */
_rc_last_valid = rc_input.timestamp_last_signal;
/* Read out values from raw message */
/* read out and scale values from raw message even if signal is invalid */
for (unsigned int i = 0; i < channel_limit; i++) {
/*
@@ -1377,127 +1382,75 @@ Sensors::rc_poll()
}
_rc.chan_count = rc_input.channel_count;
_rc.rssi = rc_input.rssi;
_rc.signal_lost = signal_lost;
_rc.timestamp = rc_input.timestamp_last_signal;
manual_control.timestamp = rc_input.timestamp_last_signal;
/* roll input - rolling right is stick-wise and rotation-wise positive */
manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled);
/*
* pitch input - stick down is negative, but stick down is pitching up (pos) in NED,
* so reverse sign.
*/
manual_control.pitch = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled);
/* yaw input - stick right is positive and positive rotation */
manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled);
/* throttle input */
manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled;
if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;
/* scale output */
if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) {
manual_control.roll *= _parameters.rc_scale_roll;
}
if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) {
manual_control.pitch *= _parameters.rc_scale_pitch;
}
if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) {
manual_control.yaw *= _parameters.rc_scale_yaw;
}
/* flaps */
if (_rc.function[FLAPS] >= 0) {
manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled);
if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) {
manual_control.flaps *= _parameters.rc_scale_flaps;
}
}
/* mode switch input */
if (_rc.function[MODE] >= 0) {
manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
}
/* assisted switch input */
if (_rc.function[ASSISTED] >= 0) {
manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled);
}
/* mission switch input */
if (_rc.function[MISSION] >= 0) {
manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
}
/* return switch input */
if (_rc.function[RETURN] >= 0) {
manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
}
// if (_rc.function[OFFBOARD_MODE] >= 0) {
// manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
// }
/* aux functions, only assign if valid mapping is present */
if (_rc.function[AUX_1] >= 0) {
manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled);
}
if (_rc.function[AUX_2] >= 0) {
manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled);
}
if (_rc.function[AUX_3] >= 0) {
manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled);
}
if (_rc.function[AUX_4] >= 0) {
manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled);
}
/* copy from mapped manual control to control group 3 */
actuator_group_3.control[0] = manual_control.roll;
actuator_group_3.control[1] = manual_control.pitch;
actuator_group_3.control[2] = manual_control.yaw;
actuator_group_3.control[3] = manual_control.throttle;
actuator_group_3.control[4] = manual_control.flaps;
actuator_group_3.control[5] = manual_control.aux1;
actuator_group_3.control[6] = manual_control.aux2;
actuator_group_3.control[7] = manual_control.aux3;
/* check if ready for publishing */
/* publish rc_channels topic even if signal is invalid, for debug */
if (_rc_pub > 0) {
orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
} else {
/* advertise the rc topic */
_rc_pub = orb_advertise(ORB_ID(rc_channels), &_rc);
}
/* check if ready for publishing */
if (_manual_control_pub > 0) {
orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control);
if (!signal_lost) {
struct manual_control_setpoint_s manual;
memset(&manual, 0 , sizeof(manual));
} else {
_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
}
/* fill values in manual_control_setpoint topic only if signal is valid */
manual.timestamp = rc_input.timestamp_last_signal;
/* check if ready for publishing */
if (_actuator_group_3_pub > 0) {
orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3);
/* limit controls */
manual.roll = get_rc_value(ROLL, -1.0, 1.0);
manual.pitch = get_rc_value(PITCH, -1.0, 1.0);
manual.yaw = get_rc_value(YAW, -1.0, 1.0);
manual.throttle = get_rc_value(THROTTLE, 0.0, 1.0);
manual.flaps = get_rc_value(FLAPS, -1.0, 1.0);
manual.aux1 = get_rc_value(AUX_1, -1.0, 1.0);
manual.aux2 = get_rc_value(AUX_2, -1.0, 1.0);
manual.aux3 = get_rc_value(AUX_3, -1.0, 1.0);
manual.aux4 = get_rc_value(AUX_4, -1.0, 1.0);
manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0);
} else {
_actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3);
/* mode switches */
manual.mode_switch = get_rc_switch_position(MODE);
manual.assisted_switch = get_rc_switch_position(ASSISTED);
manual.mission_switch = get_rc_switch_position(MISSION);
manual.return_switch = get_rc_switch_position(RETURN);
/* publish manual_control_setpoint topic */
if (_manual_control_pub > 0) {
orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual);
} else {
_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
}
/* copy from mapped manual control to control group 3 */
struct actuator_controls_s actuator_group_3;
memset(&actuator_group_3, 0 , sizeof(actuator_group_3));
actuator_group_3.timestamp = rc_input.timestamp_last_signal;
actuator_group_3.control[0] = manual.roll;
actuator_group_3.control[1] = manual.pitch;
actuator_group_3.control[2] = manual.yaw;
actuator_group_3.control[3] = manual.throttle;
actuator_group_3.control[4] = manual.flaps;
actuator_group_3.control[5] = manual.aux1;
actuator_group_3.control[6] = manual.aux2;
actuator_group_3.control[7] = manual.aux3;
/* publish actuator_controls_3 topic */
if (_actuator_group_3_pub > 0) {
orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3);
} else {
_actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3);
}
}
}
}
void
@@ -1690,4 +1643,3 @@ int sensors_main(int argc, char *argv[])
warnx("unrecognized command");
return 1;
}
src/modules/uORB/topics/manual_control_setpoint.h
+20 -17
View File
@@ -43,6 +43,16 @@
#include <stdint.h>
#include "../uORB.h"
/**
* Switch position
*/
typedef enum {
SWITCH_POS_NONE = 0, /**< switch is not mapped */
SWITCH_POS_ON, /**< switch activated (value = 1) */
SWITCH_POS_MIDDLE, /**< middle position (value = 0) */
SWITCH_POS_OFF /**< switch not activated (value = -1) */
} switch_pos_t;
/**
* @addtogroup topics
* @{
@@ -51,32 +61,25 @@
struct manual_control_setpoint_s {
uint64_t timestamp;
float roll; /**< ailerons roll / roll rate input */
float pitch; /**< elevator / pitch / pitch rate */
float yaw; /**< rudder / yaw rate / yaw */
float throttle; /**< throttle / collective thrust / altitude */
float mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */
float return_switch; /**< land 2 position switch (mandatory): land, no effect */
float assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */
float mission_switch; /**< mission 2 position switch (optional): mission, loiter */
/**
* Any of the channels below may not be available and be set to NaN
* Any of the channels may not be available and be set to NaN
* to indicate that it does not contain valid data.
*/
// XXX needed or parameter?
//float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */
float flaps; /**< flap position */
float roll; /**< ailerons roll / roll rate input, -1..1 */
float pitch; /**< elevator / pitch / pitch rate, -1..1 */
float yaw; /**< rudder / yaw rate / yaw, -1..1 */
float throttle; /**< throttle / collective thrust / altitude, 0..1 */
float flaps; /**< flap position */
float aux1; /**< default function: camera yaw / azimuth */
float aux2; /**< default function: camera pitch / tilt */
float aux3; /**< default function: camera trigger */
float aux4; /**< default function: camera roll */
float aux5; /**< default function: payload drop */
switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */
switch_pos_t return_switch; /**< land 2 position switch (mandatory): land, no effect */
switch_pos_t assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */
switch_pos_t mission_switch; /**< mission 2 position switch (optional): mission, loiter */
}; /**< manual control inputs */
/**
src/modules/uORB/topics/rc_channels.h
+1
View File
@@ -94,6 +94,7 @@ struct rc_channels_s {
char function_name[RC_CHANNELS_FUNCTION_MAX][20];
int8_t function[RC_CHANNELS_FUNCTION_MAX];
uint8_t rssi; /**< Overall receive signal strength */
bool signal_lost; /**< control signal lost, should be checked together with topic timeout */
}; /**< radio control channels. */
/**
src/modules/uORB/topics/vehicle_status.h
-28
View File
@@ -93,29 +93,6 @@ typedef enum {
FAILSAFE_STATE_MAX
} failsafe_state_t;
typedef enum {
MODE_SWITCH_MANUAL = 0,
MODE_SWITCH_ASSISTED,
MODE_SWITCH_AUTO
} mode_switch_pos_t;
typedef enum {
ASSISTED_SWITCH_SEATBELT = 0,
ASSISTED_SWITCH_EASY
} assisted_switch_pos_t;
typedef enum {
RETURN_SWITCH_NONE = 0,
RETURN_SWITCH_NORMAL,
RETURN_SWITCH_RETURN
} return_switch_pos_t;
typedef enum {
MISSION_SWITCH_NONE = 0,
MISSION_SWITCH_LOITER,
MISSION_SWITCH_MISSION
} mission_switch_pos_t;
enum VEHICLE_MODE_FLAG {
VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64,
@@ -187,11 +164,6 @@ struct vehicle_status_s {
bool is_rotary_wing;
mode_switch_pos_t mode_switch;
return_switch_pos_t return_switch;
assisted_switch_pos_t assisted_switch;
mission_switch_pos_t mission_switch;
bool condition_battery_voltage_valid;
bool condition_system_in_air_restore; /**< true if we can restore in mid air */
bool condition_system_sensors_initialized;