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

mode switching for all platforms, additional fixed wing modes

Browse Source
This commit is contained in:
Lorenz Meier
2012-11-25 13:55:28 +01:00
parent dc72d467d4
commit faa672f8bb
5 changed files with 215 additions and 132 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/commander/commander.c
+12 -6
View File
@@ -1496,14 +1496,20 @@ int commander_thread_main(int argc, char *argv[])
} }
//printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count); //printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count);
if (sp_man.override_mode_switch > STICK_ON_OFF_LIMIT) { if (sp_man.aux1_cam_pan_flaps < -STICK_ON_OFF_LIMIT) {
update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd); update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
} else if (sp_man.override_mode_switch < -STICK_ON_OFF_LIMIT) {
update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
} else { } else {
update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd); if (sp_man.override_mode_switch > STICK_ON_OFF_LIMIT) {
//update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
// XXX hack
update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
} else if (sp_man.override_mode_switch < -STICK_ON_OFF_LIMIT) {
update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
} else {
update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
}
} }
/* handle the case where RC signal was regained */ /* handle the case where RC signal was regained */
apps/fixedwing_att_control/fixedwing_att_control_main.c
+46 -12
View File
@@ -62,8 +62,8 @@
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
#include <systemlib/pid/pid.h> #include <systemlib/pid/pid.h>
#include <systemlib/geo/geo.h> #include <systemlib/geo/geo.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h> #include <systemlib/systemlib.h>
#include <fixedwing_att_control_rate.h> #include <fixedwing_att_control_rate.h>
#include <fixedwing_att_control_att.h> #include <fixedwing_att_control_att.h>
@@ -126,7 +126,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
} }
/* welcome user */ /* welcome user */
printf("[fixedwing att_control] started\n"); printf("[fixedwing att control] started\n");
/* declare and safely initialize all structs */ /* declare and safely initialize all structs */
struct vehicle_attitude_s att; struct vehicle_attitude_s att;
@@ -153,6 +153,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
actuators.control[i] = 0.0f; actuators.control[i] = 0.0f;
} }
orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
// orb_advert_t debug_pub = orb_advertise(ORB_ID(debug_key_value), &debug_output); // orb_advert_t debug_pub = orb_advertise(ORB_ID(debug_key_value), &debug_output);
// debug_output.key[0] = '1'; // debug_output.key[0] = '1';
@@ -185,19 +186,51 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
/* Control */ /* Control */
if (vstatus.state_machine == SYSTEM_STATE_AUTO) { if (vstatus.state_machine == SYSTEM_STATE_AUTO) {
/* Attitude Control */ /* attitude control */
fixedwing_att_control_attitude(&att_sp, fixedwing_att_control_attitude(&att_sp, &att, &rates_sp);
&att, /* publish rate setpoint */
&rates_sp); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
/* Attitude Rate Control */ /* angular rate control */
fixedwing_att_control_rates(&rates_sp, gyro, &actuators); fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
//REMOVEME XXX /* pass through throttle */
actuators.control[3] = 0.7f; actuators.control[3] = att_sp.thrust;
} else if (vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if(vstatus.rc_signal_lost) {
// XXX define failsafe throttle param
//param_get(failsafe_throttle_handle, &failsafe_throttle);
att_sp.roll_body = 0.3f;
att_sp.pitch_body = 0.0f;
att_sp.thrust = 0.5f;
// XXX disable yaw control, loiter
} else {
att_sp.roll_body = manual_sp.roll;
att_sp.pitch_body = manual_sp.pitch;
att_sp.yaw_body = 0;
att_sp.thrust = manual_sp.throttle;
att_sp.timestamp = hrt_absolute_time();
}
fixedwing_att_control_attitude(&att_sp, &att, &rates_sp);
/* publish rate setpoint */
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
/* angular rate control */
fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
/* pass through throttle */
actuators.control[3] = att_sp.thrust;
// debug_output.value = rates_sp.pitch;
// orb_publish(ORB_ID(debug_key_value), debug_pub, &debug_output);
} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
/* directly pass through values */ /* directly pass through values */
actuators.control[0] = manual_sp.roll; actuators.control[0] = manual_sp.roll;
@@ -224,6 +257,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
close(att_sub); close(att_sub);
close(actuator_pub); close(actuator_pub);
close(rates_pub);
fflush(stdout); fflush(stdout);
exit(0); exit(0);
@@ -268,7 +302,7 @@ int fixedwing_att_control_main(int argc, char *argv[])
deamon_task = task_spawn("fixedwing_att_control", deamon_task = task_spawn("fixedwing_att_control",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20, SCHED_PRIORITY_MAX - 20,
4096, 2048,
fixedwing_att_control_thread_main, fixedwing_att_control_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL); (argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true; thread_running = true;
apps/fixedwing_att_control/fixedwing_att_control_rate.c
+5 -3
View File
@@ -1,7 +1,7 @@
/**************************************************************************** /****************************************************************************
* *
* Copyright (C) 2012 PX4 Development Team. All rights reserved. * Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> * Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@@ -34,6 +34,8 @@
/** /**
* @file fixedwing_att_control_rate.c * @file fixedwing_att_control_rate.c
* Implementation of a fixed wing attitude controller. * Implementation of a fixed wing attitude controller.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
*/ */
#include <fixedwing_att_control_rate.h> #include <fixedwing_att_control_rate.h>
@@ -171,8 +173,8 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0, deltaT); actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0, deltaT);
actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0, deltaT); //TODO: (-) sign comes from the elevator (positive --> deflection downwards), this has to be moved to the mixer... actuators->control[1] = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0, deltaT);
actuators->control[2] = 0;//pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0, deltaT); //XXX TODO disabled for now actuators->control[2] = 0.0f;//pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0, deltaT); //XXX TODO disabled for now
counter++; counter++;
apps/fixedwing_pos_control/fixedwing_pos_control_main.c
+136 -110
View File
@@ -57,9 +57,11 @@
#include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/parameter_update.h>
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
#include <systemlib/pid/pid.h> #include <systemlib/pid/pid.h>
#include <systemlib/geo/geo.h> #include <systemlib/geo/geo.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h> #include <systemlib/systemlib.h>
/* /*
@@ -162,16 +164,16 @@ static int parameters_update(const struct fw_pos_control_param_handles *h, struc
int fixedwing_pos_control_thread_main(int argc, char *argv[]) int fixedwing_pos_control_thread_main(int argc, char *argv[])
{ {
/* read arguments */ /* read arguments */
bool verbose = false; // bool verbose = false;
for (int i = 1; i < argc; i++) { // for (int i = 1; i < argc; i++) {
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) { // if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
verbose = true; // verbose = true;
} // }
} // }
/* welcome user */ /* welcome user */
printf("[fixedwing att_control] started\n"); printf("[fixedwing pos control] started\n");
/* declare and safely initialize all structs */ /* declare and safely initialize all structs */
struct vehicle_global_position_s global_pos; struct vehicle_global_position_s global_pos;
@@ -184,6 +186,8 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
memset(&att, 0, sizeof(att)); memset(&att, 0, sizeof(att));
struct crosstrack_error_s xtrack_err; struct crosstrack_error_s xtrack_err;
memset(&xtrack_err, 0, sizeof(xtrack_err)); memset(&xtrack_err, 0, sizeof(xtrack_err));
struct parameter_update_s param_update;
memset(&param_update, 0, sizeof(param_update));
/* output structs */ /* output structs */
struct vehicle_attitude_setpoint_s attitude_setpoint; struct vehicle_attitude_setpoint_s attitude_setpoint;
@@ -193,129 +197,151 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
attitude_setpoint.roll_body = 0.0f; attitude_setpoint.roll_body = 0.0f;
attitude_setpoint.pitch_body = 0.0f; attitude_setpoint.pitch_body = 0.0f;
attitude_setpoint.yaw_body = 0.0f; attitude_setpoint.yaw_body = 0.0f;
attitude_setpoint.thrust = 0.0f;
orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint); orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint);
/* subscribe */ /* subscribe */
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int param_sub = orb_subscribe(ORB_ID(parameter_update));
/* Setup of loop */ /* Setup of loop */
struct pollfd fds = { .fd = att_sub, .events = POLLIN }; struct pollfd fds[2] = {
{ .fd = param_sub, .events = POLLIN },
{ .fd = att_sub, .events = POLLIN }
};
bool global_sp_updated_set_once = false; bool global_sp_updated_set_once = false;
float psi_track = 0.0f; float psi_track = 0.0f;
int counter = 0;
struct fw_pos_control_params p;
struct fw_pos_control_param_handles h;
PID_t heading_controller;
PID_t altitude_controller;
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f,p.roll_lim,PID_MODE_DERIVATIV_NONE);
pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f,p.pitch_lim,PID_MODE_DERIVATIV_NONE);
/* error and performance monitoring */
perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval");
perf_counter_t fw_err_perf = perf_alloc(PC_COUNT, "fixedwing_pos_control_err");
while(!thread_should_exit) while(!thread_should_exit)
{ {
/* wait for a sensor update, check for exit condition every 500 ms */ /* wait for a sensor update, check for exit condition every 500 ms */
poll(&fds, 1, 500); int ret = poll(fds, 2, 500);
static int counter = 0; if (ret < 0) {
static bool initialized = false; /* poll error, count it in perf */
perf_count(fw_err_perf);
} else if (ret == 0) {
/* no return value, ignore */
} else {
static struct fw_pos_control_params p; /* only update parameters if they changed */
static struct fw_pos_control_param_handles h; if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag */
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), param_sub, &update);
PID_t heading_controller; /* update parameters from storage */
PID_t altitude_controller; parameters_update(&h, &p);
pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, p.roll_lim);
if(!initialized) pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim);
{
parameters_init(&h);
parameters_update(&h, &p);
pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f,p.roll_lim,PID_MODE_DERIVATIV_NONE);
pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f,p.pitch_lim,PID_MODE_DERIVATIV_NONE);
initialized = true;
}
/* load new parameters with lower rate */
if (counter % 100 == 0) {
/* update parameters from storage */
parameters_update(&h, &p);
pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, p.roll_lim);
pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim);
}
/* Check if there is a new position or setpoint */
bool pos_updated;
orb_check(global_pos_sub, &pos_updated);
bool global_sp_updated;
orb_check(global_setpoint_sub, &global_sp_updated);
/* Load local copies */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
if(pos_updated)
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
if (global_sp_updated)
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
if(global_sp_updated) {
start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint)
global_sp_updated_set_once = true;
psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
printf("psi_track: %0.4f\n", (double)psi_track);
}
/* Control */
/* Simple Horizontal Control */
if(global_sp_updated_set_once)
{
// if (counter % 100 == 0)
// printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon);
/* calculate crosstrack error */
// Only the case of a straight line track following handled so far
int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
(double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d,
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
if(!(distance_res != OK || xtrack_err.past_end)) {
float delta_psi_c = -p.xtrack_p * xtrack_err.distance; //(-) because z axis points downwards
if(delta_psi_c > 60.0f*M_DEG_TO_RAD_F)
delta_psi_c = 60.0f*M_DEG_TO_RAD_F;
if(delta_psi_c < -60.0f*M_DEG_TO_RAD_F)
delta_psi_c = -60.0f*M_DEG_TO_RAD_F;
float psi_c = psi_track + delta_psi_c;
float psi_e = psi_c - att.yaw;
/* shift error to prevent wrapping issues */
psi_e = _wrap_pi(psi_e);
/* calculate roll setpoint, do this artificially around zero */
attitude_setpoint.roll_body = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f);
// if (counter % 100 == 0)
// printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c);
}
else {
if (counter % 100 == 0)
printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end);
} }
/* only run controller if attitude changed */
if (fds[1].revents & POLLIN) {
/* check if there is a new position or setpoint */
bool pos_updated;
orb_check(global_pos_sub, &pos_updated);
bool global_sp_updated;
orb_check(global_setpoint_sub, &global_sp_updated);
/* load local copies */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
if (pos_updated) {
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
}
if (global_sp_updated) {
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint)
global_sp_updated_set_once = true;
psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
printf("psi_track: %0.4f\n", (double)psi_track);
}
/* simple horizontal control, execute if line between wps is known */
if(global_sp_updated_set_once)
{
// if (counter % 100 == 0)
// printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon);
/* calculate crosstrack error */
// Only the case of a straight line track following handled so far
int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
(double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d,
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
if(!(distance_res != OK || xtrack_err.past_end)) {
float delta_psi_c = -p.xtrack_p * xtrack_err.distance; //(-) because z axis points downwards
if(delta_psi_c > 60.0f*M_DEG_TO_RAD_F)
delta_psi_c = 60.0f*M_DEG_TO_RAD_F;
if(delta_psi_c < -60.0f*M_DEG_TO_RAD_F)
delta_psi_c = -60.0f*M_DEG_TO_RAD_F;
float psi_c = psi_track + delta_psi_c;
float psi_e = psi_c - att.yaw;
/* shift error to prevent wrapping issues */
psi_e = _wrap_pi(psi_e);
/* calculate roll setpoint, do this artificially around zero */
attitude_setpoint.roll_body = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f);
// if (counter % 100 == 0)
// printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n",xtrack_err.distance, delta_psi_c);
}
else {
if (counter % 100 == 0)
printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end);
}
// XXX SIMPLE ALTITUDE, BUT NO SPEED CONTROL
if (pos_updated) {
//TODO: take care of relative vs. ab. altitude
attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
}
// XXX need speed control
attitude_setpoint.thrust = 0.7f;
/* publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint);
/* measure in what intervals the controller runs */
perf_count(fw_interval_perf);
} else {
// XXX no setpoint, decent default needed (loiter?)
}
}
} }
/* Very simple Altitude Control */
if(global_sp_updated_set_once && pos_updated)
{
//TODO: take care of relative vs. ab. altitude
attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
}
/*Publish the attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint);
counter++;
} }
printf("[fixedwing_pos_control] exiting.\n"); printf("[fixedwing_pos_control] exiting.\n");
@@ -367,7 +393,7 @@ int fixedwing_pos_control_main(int argc, char *argv[])
deamon_task = task_spawn("fixedwing_pos_control", deamon_task = task_spawn("fixedwing_pos_control",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20, SCHED_PRIORITY_MAX - 20,
4096, 2048,
fixedwing_pos_control_thread_main, fixedwing_pos_control_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL); (argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true; thread_running = true;
apps/sensors/sensors.cpp
+16 -1
View File
@@ -973,7 +973,7 @@ Sensors::ppm_poll()
} }
/* yaw input - stick right is positive and positive rotation */ /* yaw input - stick right is positive and positive rotation */
manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled * _parameters.rc_scale_yaw; manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled;
if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f; if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f;
if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f; if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f;
if (!isnan(_parameters.rc_scale_yaw) || !isinf(_parameters.rc_scale_yaw)) { if (!isnan(_parameters.rc_scale_yaw) || !isinf(_parameters.rc_scale_yaw)) {
@@ -990,6 +990,21 @@ Sensors::ppm_poll()
if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f; if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f;
if (manual_control.override_mode_switch > 1.0f) manual_control.override_mode_switch = 1.0f; if (manual_control.override_mode_switch > 1.0f) manual_control.override_mode_switch = 1.0f;
/* aux inputs */
manual_control.aux1_cam_pan_flaps = _rc.chan[_rc.function[FUNC_0]].scaled;
if (manual_control.aux1_cam_pan_flaps < -1.0f) manual_control.aux1_cam_pan_flaps = -1.0f;
if (manual_control.aux1_cam_pan_flaps > 1.0f) manual_control.aux1_cam_pan_flaps = 1.0f;
/* aux inputs */
manual_control.aux2_cam_tilt = _rc.chan[_rc.function[FUNC_1]].scaled;
if (manual_control.aux2_cam_tilt < -1.0f) manual_control.aux2_cam_tilt = -1.0f;
if (manual_control.aux2_cam_tilt > 1.0f) manual_control.aux2_cam_tilt = 1.0f;
/* aux inputs */
manual_control.aux3_cam_zoom = _rc.chan[_rc.function[FUNC_2]].scaled;
if (manual_control.aux3_cam_zoom < -1.0f) manual_control.aux3_cam_zoom = -1.0f;
if (manual_control.aux3_cam_zoom > 1.0f) manual_control.aux3_cam_zoom = 1.0f;
orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control);
} }