mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-25 01:23:35 +08:00
Kicked out mix_and_link, deleted old MPU driver, disabled (still needed for reference) old HMC and MS5611 drivers. Removed driver init from up_nsh.c. Reworked fixedwing_control to be closer to up-to-date api, still more clean up needed. Fixed a bug that limited the motor thrust for multirotor control
This commit is contained in:
Lorenz Meier
@@ -52,14 +52,15 @@
|
||||
#include <arch/board/board.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
#include <nuttx/spi.h>
|
||||
#include "../mix_and_link/mix_and_link.h" //TODO: add to Makefile
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/rc_channels.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_global_position_setpoint.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/fixedwing_control.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
__EXPORT int fixedwing_control_main(int argc, char *argv[]);
|
||||
@@ -710,19 +711,38 @@ int fixedwing_control_main(int argc, char *argv[])
|
||||
/* welcome user */
|
||||
printf("[fixedwing control] started\n");
|
||||
|
||||
/* Set up to publish fixed wing control messages */
|
||||
struct fixedwing_control_s control;
|
||||
orb_advert_t fixedwing_control_pub = orb_advertise(ORB_ID(fixedwing_control), &control);
|
||||
/* output structs */
|
||||
struct actuator_controls_s actuators;
|
||||
struct actuator_armed_s armed;
|
||||
struct vehicle_attitude_setpoint_s att_sp;
|
||||
memset(&att_sp, 0, sizeof(att_sp));
|
||||
|
||||
/* publish actuator controls */
|
||||
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
|
||||
actuators.control[i] = 0.0f;
|
||||
orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
|
||||
armed.armed = false;
|
||||
orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
|
||||
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
|
||||
|
||||
/* Subscribe to global position, attitude and rc */
|
||||
struct vehicle_global_position_s global_pos;
|
||||
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
struct vehicle_attitude_s att;
|
||||
int attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
struct rc_channels_s rc;
|
||||
int rc_sub = orb_subscribe(ORB_ID(rc_channels));
|
||||
struct vehicle_global_position_setpoint_s global_setpoint;
|
||||
int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
|
||||
/* declare and safely initialize all structs */
|
||||
struct vehicle_status_s state;
|
||||
memset(&state, 0, sizeof(state));
|
||||
struct vehicle_attitude_s att;
|
||||
memset(&att_sp, 0, sizeof(att_sp));
|
||||
struct manual_control_setpoint_s manual;
|
||||
memset(&manual, 0, sizeof(manual));
|
||||
|
||||
/* subscribe to attitude, motor setpoints and system state */
|
||||
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
|
||||
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
|
||||
/* Mainloop setup */
|
||||
unsigned int loopcounter = 0;
|
||||
@@ -734,44 +754,6 @@ int fixedwing_control_main(int argc, char *argv[])
|
||||
|
||||
/* Servo setup */
|
||||
|
||||
int fd;
|
||||
servo_position_t data[PWM_OUTPUT_MAX_CHANNELS];
|
||||
|
||||
fd = open("/dev/pwm_servo", O_RDWR);
|
||||
|
||||
if (fd < 0) {
|
||||
printf("[fixedwing control] Failed opening /dev/pwm_servo\n");
|
||||
}
|
||||
|
||||
ioctl(fd, PWM_SERVO_ARM, 0);
|
||||
|
||||
int16_t buffer_rc[3];
|
||||
int16_t buffer_servo[3];
|
||||
mixer_data_t mixer_buffer;
|
||||
mixer_buffer.input = buffer_rc;
|
||||
mixer_buffer.output = buffer_servo;
|
||||
|
||||
mixer_conf_t mixers[3];
|
||||
|
||||
mixers[0].source = PITCH;
|
||||
mixers[0].nr_actuators = 2;
|
||||
mixers[0].dest[0] = AIL_1;
|
||||
mixers[0].dest[1] = AIL_2;
|
||||
mixers[0].dual_rate[0] = 1;
|
||||
mixers[0].dual_rate[1] = 1;
|
||||
|
||||
mixers[1].source = ROLL;
|
||||
mixers[1].nr_actuators = 2;
|
||||
mixers[1].dest[0] = AIL_1;
|
||||
mixers[1].dest[1] = AIL_2;
|
||||
mixers[1].dual_rate[0] = 1;
|
||||
mixers[1].dual_rate[1] = -1;
|
||||
|
||||
mixers[2].source = THROTTLE;
|
||||
mixers[2].nr_actuators = 1;
|
||||
mixers[2].dest[0] = MOT;
|
||||
mixers[2].dual_rate[0] = 1;
|
||||
|
||||
/*
|
||||
* Main control, navigation and servo routine
|
||||
*/
|
||||
@@ -786,8 +768,15 @@ int fixedwing_control_main(int argc, char *argv[])
|
||||
// XXX add error checking
|
||||
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
|
||||
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
|
||||
orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &att);
|
||||
orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
|
||||
|
||||
/* get a local copy of system state */
|
||||
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
|
||||
/* get a local copy of manual setpoint */
|
||||
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
|
||||
/* get a local copy of attitude */
|
||||
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
|
||||
/* get a local copy of attitude setpoint */
|
||||
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
|
||||
|
||||
/* scaling factors are defined by the data from the APM Planner
|
||||
* TODO: ifdef for other parameters (HIL/Real world switch)
|
||||
@@ -810,20 +799,19 @@ int fixedwing_control_main(int argc, char *argv[])
|
||||
plane_data.yawspeed = att.yawspeed;
|
||||
|
||||
/* parameter values */
|
||||
get_parameters(&plane_data);
|
||||
if (loopcounter % 20 == 0) {
|
||||
get_parameters(&plane_data);
|
||||
}
|
||||
|
||||
/* Attitude control part */
|
||||
|
||||
if (verbose && loopcounter % 20 == 0) {
|
||||
/******************************** DEBUG OUTPUT ************************************************************/
|
||||
|
||||
printf("Parameter: %i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i \n", (int)plane_data.Kp_att, (int)plane_data.Ki_att,
|
||||
printf("Parameter: %i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i\n", (int)plane_data.Kp_att, (int)plane_data.Ki_att,
|
||||
(int)plane_data.Kd_att, (int)plane_data.intmax_att, (int)plane_data.Kp_pos, (int)plane_data.Ki_pos,
|
||||
(int)plane_data.Kd_pos, (int)plane_data.intmax_pos, (int)plane_data.airspeed,
|
||||
(int)plane_data.wp_x, (int)plane_data.wp_y, (int)plane_data.wp_z,
|
||||
(int)global_data_parameter_storage->pm.param_values[PARAM_WPLON],
|
||||
(int)global_data_parameter_storage->pm.param_values[PARAM_WPLAT],
|
||||
(int)global_data_parameter_storage->pm.param_values[PARAM_WPALT]);
|
||||
(int)plane_data.wp_x, (int)plane_data.wp_y, (int)plane_data.wp_z);
|
||||
|
||||
printf("PITCH SETPOINT: %i\n", (int)(100.0f*plane_data.pitch_setpoint));
|
||||
printf("ROLL SETPOINT: %i\n", (int)(100.0f*plane_data.roll_setpoint));
|
||||
@@ -840,20 +828,13 @@ int fixedwing_control_main(int argc, char *argv[])
|
||||
// printf("\nBody Rates: \n P: %i \n Q: %i \n R: %i \n",
|
||||
// (int)(10000 * plane_data.p), (int)(10000 * plane_data.q), (int)(10000 * plane_data.r));
|
||||
|
||||
printf("\nCalculated outputs: \n R: %i\n P: %i\n Y: %i\n T: %i \n",
|
||||
(int)(10000.0f * control_outputs.roll_ailerons), (int)(10000.0f * control_outputs.pitch_elevator),
|
||||
(int)(10000.0f * control_outputs.yaw_rudder), (int)(10000.0f * control_outputs.throttle));
|
||||
printf("\nCalculated outputs: \n R: %8.4f\n P: %8.4f\n Y: %8.4f\n T: %8.4f \n",
|
||||
att_sp.roll_body, att_sp.pitch_body,
|
||||
att_sp.yaw_body, att_sp.thrust);
|
||||
|
||||
/************************************************************************************************************/
|
||||
}
|
||||
|
||||
/*
|
||||
* Computation section
|
||||
*
|
||||
* The function calls to compute the required control values happen
|
||||
* in this section.
|
||||
*/
|
||||
|
||||
/* Set plane mode */
|
||||
set_plane_mode();
|
||||
|
||||
@@ -864,123 +845,37 @@ int fixedwing_control_main(int argc, char *argv[])
|
||||
/* Calculate the body frame angles of the aircraft */
|
||||
//calc_bodyframe_angles(plane_data.roll,plane_data.pitch,plane_data.yaw);
|
||||
|
||||
/* Calculate the output values */
|
||||
control_outputs.roll_ailerons = calc_roll_ail();
|
||||
control_outputs.pitch_elevator = calc_pitch_elev();
|
||||
//control_outputs.yaw_rudder = calc_yaw_rudder();
|
||||
control_outputs.throttle = calc_throttle();
|
||||
// if (manual.override_mode_switch < XXX) {
|
||||
|
||||
if (rc.chan[rc.function[OVERRIDE]].scale < MANUAL) { // if we're flying in automated mode
|
||||
|
||||
/*
|
||||
* TAKEOFF hack for HIL
|
||||
*/
|
||||
if (plane_data.mode == TAKEOFF) {
|
||||
control.attitude_control_output[ROLL] = 0;
|
||||
control.attitude_control_output[PITCH] = 5000;
|
||||
control.attitude_control_output[THROTTLE] = 10000;
|
||||
//global_data_fixedwing_control->attitude_control_output[YAW] = (int16_t)(control_outputs.yaw_rudder);
|
||||
}
|
||||
|
||||
if (plane_data.mode == CRUISE) {
|
||||
control.attitude_control_output[ROLL] = control_outputs.roll_ailerons;
|
||||
control.attitude_control_output[PITCH] = control_outputs.pitch_elevator;
|
||||
control.attitude_control_output[THROTTLE] = control_outputs.throttle;
|
||||
//control->attitude_control_output[YAW] = (int16_t)(control_outputs.yaw_rudder);
|
||||
}
|
||||
|
||||
control.counter++;
|
||||
control.timestamp = hrt_absolute_time();
|
||||
}
|
||||
|
||||
/* Navigation part */
|
||||
|
||||
// Get GPS Waypoint
|
||||
// Get Waypoint
|
||||
|
||||
// if(global_data_wait(&global_data_position_setpoint->access_conf) == 0)
|
||||
// {
|
||||
// plane_data.wp_x = global_data_position_setpoint->x;
|
||||
// plane_data.wp_y = global_data_position_setpoint->y;
|
||||
// plane_data.wp_z = global_data_position_setpoint->z;
|
||||
// }
|
||||
// global_data_unlock(&global_data_position_setpoint->access_conf);
|
||||
// XXX FIXME
|
||||
|
||||
if (rc.chan[rc.function[OVERRIDE]].scale < MANUAL) { // AUTO mode
|
||||
// AUTO/HYBRID switch
|
||||
//else if (manual.override_mode_switch > MANUAL) { // AUTO mode
|
||||
|
||||
/* calculate setpoint based on current position and waypoint */
|
||||
att_sp.roll_body = calc_roll_setpoint();
|
||||
att_sp.pitch_body = calc_pitch_setpoint();
|
||||
att_sp.thrust = calc_throttle_setpoint();
|
||||
|
||||
if (rc.chan[rc.function[OVERRIDE]].scale < AUTO) {
|
||||
plane_data.roll_setpoint = calc_roll_setpoint();
|
||||
plane_data.pitch_setpoint = calc_pitch_setpoint();
|
||||
plane_data.throttle_setpoint = calc_throttle_setpoint();
|
||||
/* calculate the control outputs based on roll / pitch / yaw setpoints */
|
||||
actuators.control[0] = calc_roll_ail();
|
||||
actuators.control[1] = calc_pitch_elev();
|
||||
actuators.control[2] = calc_yaw_rudder(att.yaw);
|
||||
actuators.control[3] = calc_throttle();
|
||||
|
||||
} else {
|
||||
plane_data.roll_setpoint = rc.chan[rc.function[ROLL]].scale / 200;
|
||||
plane_data.pitch_setpoint = rc.chan[rc.function[PITCH]].scale / 200;
|
||||
plane_data.throttle_setpoint = rc.chan[rc.function[THROTTLE]].scale / 200;
|
||||
}
|
||||
/* publish attitude setpoint (for MAVLink) */
|
||||
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
|
||||
|
||||
//control_outputs.yaw_rudder = calc_yaw_rudder(plane_data.hdg);
|
||||
/* publish actuator setpoints (for mixer) */
|
||||
|
||||
} else {
|
||||
control.attitude_control_output[ROLL] = rc.chan[rc.function[ROLL]].scale/10000;
|
||||
control.attitude_control_output[PITCH] = rc.chan[rc.function[PITCH]].scale/10000;
|
||||
control.attitude_control_output[THROTTLE] = rc.chan[rc.function[THROTTLE]].scale/10000;
|
||||
// since we don't have a yaw rudder
|
||||
//control->attitude_control_output[3] = global_data_rc_channels->chan[YAW].scale;
|
||||
|
||||
control.counter++;
|
||||
control.timestamp = hrt_absolute_time();
|
||||
}
|
||||
|
||||
/* publish the control data */
|
||||
|
||||
orb_publish(ORB_ID(fixedwing_control), fixedwing_control_pub, &control);
|
||||
|
||||
/* Servo part */
|
||||
|
||||
buffer_rc[ROLL] = (int16_t)(10000*control.attitude_control_output[ROLL]);
|
||||
buffer_rc[PITCH] = (int16_t)(10000*control.attitude_control_output[PITCH]);
|
||||
buffer_rc[THROTTLE] = (int16_t)(10000*control.attitude_control_output[THROTTLE]);
|
||||
|
||||
//mix_and_link(mixers, 3, 2, &mixer_buffer);
|
||||
|
||||
// Scaling and saturation of servo outputs happens here
|
||||
|
||||
data[AIL_1] = buffer_servo[AIL_1] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE]
|
||||
+ global_data_parameter_storage->pm.param_values[PARAM_SERVO1_TRIM];
|
||||
|
||||
if (data[AIL_1] > SERVO_MAX)
|
||||
data[AIL_1] = SERVO_MAX;
|
||||
|
||||
if (data[AIL_1] < SERVO_MIN)
|
||||
data[AIL_1] = SERVO_MIN;
|
||||
|
||||
data[AIL_2] = buffer_servo[AIL_2] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE]
|
||||
+ global_data_parameter_storage->pm.param_values[PARAM_SERVO2_TRIM];
|
||||
|
||||
if (data[AIL_2] > SERVO_MAX)
|
||||
data[AIL_2] = SERVO_MAX;
|
||||
|
||||
if (data[AIL_2] < SERVO_MIN)
|
||||
data[AIL_2] = SERVO_MIN;
|
||||
|
||||
data[MOT] = buffer_servo[MOT] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE]
|
||||
+ global_data_parameter_storage->pm.param_values[PARAM_SERVO3_TRIM];
|
||||
|
||||
if (data[MOT] > SERVO_MAX)
|
||||
data[MOT] = SERVO_MAX;
|
||||
|
||||
if (data[MOT] < SERVO_MIN)
|
||||
data[MOT] = SERVO_MIN;
|
||||
|
||||
int result = write(fd, &data, sizeof(data));
|
||||
|
||||
if (result != sizeof(data)) {
|
||||
if (failcounter < 10 || failcounter % 20 == 0) {
|
||||
printf("[fixedwing_control] failed writing servo outputs\n");
|
||||
}
|
||||
failcounter++;
|
||||
}
|
||||
/* arming state depends on commander arming state */
|
||||
armed.armed = state.flag_system_armed;
|
||||
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
|
||||
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
|
||||
|
||||
loopcounter++;
|
||||
|
||||
|
||||
@@ -1,38 +0,0 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (C) 2012 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
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# Makefile to build the mix-and-link functions
|
||||
#
|
||||
|
||||
include $(APPDIR)/mk/app.mk
|
||||
@@ -1,81 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Nils Wenzler <wenzlern@ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/*
|
||||
* @file Mixing / linking of RC channels
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <unistd.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "mix_and_link.h"
|
||||
|
||||
/*
|
||||
* Library function that mixes the abstract functions like ROLL, PITCH, THROTTLE
|
||||
* to one or several actuators.
|
||||
* @param *mixers is a pointer to the configuration struct, declared in mix_and_link.h
|
||||
* @param nr_mixers is a integer that denotes the number of mixers that should be processed
|
||||
* @param nr_actuators is a integer that denotes the max number of actuators that get linked to one source
|
||||
* @param *data is a pointer to the mixer data struct, declared in mix_and_link.h
|
||||
*/
|
||||
void mix_and_link(const mixer_conf_t *mixers, uint8_t nr_mixers, uint8_t nr_actuators, mixer_data_t *data)
|
||||
{
|
||||
|
||||
/* Reset the Output Array */
|
||||
uint8_t i, j;
|
||||
|
||||
for (i = 0; i < nr_actuators; i++) {
|
||||
data->output[i] = 0;
|
||||
}
|
||||
|
||||
/* Loop throug the given mixers */
|
||||
for (i = 0; i < nr_mixers; i++) {
|
||||
|
||||
/* The actual mixing */
|
||||
for (j = 0; j < mixers[i].nr_actuators; j++) {
|
||||
data->output[mixers[i].dest[j]] += (int16_t)(data->input[mixers[i].source]
|
||||
* mixers[i].dual_rate[j]);
|
||||
|
||||
/* Saturate to +-10000 */
|
||||
if (data->output[mixers[i].dest[j]] > 10000)
|
||||
data->output[mixers[i].dest[j]] = 10000;
|
||||
else if (data->output[mixers[i].dest[j]] < -10000)
|
||||
data->output[mixers[i].dest[j]] = -10000;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -1,115 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Nils Wenzler <wenzlern@ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
#ifndef _MIX_AND_LINK_H_
|
||||
#define _MIX_AND_LINK_H_
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#include <unistd.h>
|
||||
#include <stdint.h>
|
||||
|
||||
/**< The maximum number of actuators that get linked to one abstract function*/
|
||||
#define NR_MAX_ACTUATORS 2
|
||||
|
||||
/**
|
||||
* The struct mixer_conf_t contains all the necessary information for one mixer.
|
||||
* Make an array of type mixer_conf_t for several mix functions.
|
||||
*/
|
||||
typedef struct {
|
||||
uint8_t source; /**< source RC channel index 0..n */
|
||||
uint8_t nr_actuators; /**< number of actuators to output to */
|
||||
uint8_t dest[NR_MAX_ACTUATORS]; /**< Which actuators to output to */
|
||||
float dual_rate[NR_MAX_ACTUATORS]; /**< Direction and rate of mixing. Range [-1,1] */
|
||||
} mixer_conf_t; /**< Setup for one mixer */
|
||||
|
||||
/*
|
||||
* The struct mixer_data contains two int16_t arrays with the abstract source functions
|
||||
* (Throttle, Roll,...) in the input, and an empty array output of the desired
|
||||
* size for the results.
|
||||
*/
|
||||
typedef struct {
|
||||
int16_t *input;
|
||||
int16_t *output;
|
||||
} mixer_data_t;
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
|
||||
/*
|
||||
* Mixes the functions to the actuators.
|
||||
* e.g. Throttle, Roll,...
|
||||
*
|
||||
* Each mixer_conf struct in the mixers array contains the source function (ex. THROTTLE), the number
|
||||
* of actuators, an array with the desired actuators, Dual Rate determines which
|
||||
* percentage of the source gets mixed to the destination and sets the direction.
|
||||
* It's range is between -1 and 1.
|
||||
*
|
||||
* EXAMPLE: Deltamix for a wing plane:
|
||||
*
|
||||
* mixer_data_t mixer_buffer;
|
||||
* mixer_buffer.input = buffer_rc;
|
||||
* mixer_buffer.output = buffer_servo;
|
||||
*
|
||||
*
|
||||
* mixer_conf_t mixers[3];
|
||||
*
|
||||
* mixers[0].source = PITCH;
|
||||
* mixers[0].nr_actuators = 2;
|
||||
* mixers[0].dest[0] = AIL_1;
|
||||
* mixers[0].dest[1] = AIL_2;
|
||||
* mixers[0].dual_rate[0] = 1;
|
||||
* mixers[0].dual_rate[1] = 1;
|
||||
*
|
||||
* mixers[1].source = ROLL;
|
||||
* mixers[1].nr_actuators = 2;
|
||||
* mixers[1].dest[0] = AIL_1;
|
||||
* mixers[1].dest[1] = AIL_2;
|
||||
* mixers[1].dual_rate[0] = 1;
|
||||
* mixers[1].dual_rate[0] = -1;
|
||||
*
|
||||
* mixers[2].source = THROTTLE;
|
||||
* mixers[2].nr_actuators = 1;
|
||||
* mixers[2].dest[0] = MOT;
|
||||
* mixers[2].dual_rate[0] = 1;
|
||||
*
|
||||
* @param mixers pointer to the mixer struct array
|
||||
* @param nr_mixers number of mixers in struct array
|
||||
* @param nr_actuators number of actuators
|
||||
* @param data pointer to the mixer_data struct.
|
||||
*/
|
||||
void mix_and_link(const mixer_conf_t *mixers, uint8_t nr_mixers, uint8_t nr_actuators, mixer_data_t *data);
|
||||
|
||||
#endif
|
||||
@@ -96,8 +96,6 @@ mc_thread_main(int argc, char *argv[])
|
||||
memset(&raw, 0, sizeof(raw));
|
||||
struct ardrone_motors_setpoint_s setpoint;
|
||||
memset(&setpoint, 0, sizeof(setpoint));
|
||||
struct actuator_controls_s actuator_controls;
|
||||
memset(&actuator_controls, 0, sizeof(actuator_controls));
|
||||
|
||||
struct actuator_controls_s actuators;
|
||||
struct actuator_armed_s armed;
|
||||
@@ -153,12 +151,7 @@ mc_thread_main(int argc, char *argv[])
|
||||
att_sp.yaw_body = 0.0f;
|
||||
att_sp.thrust = 0.1f;
|
||||
} else {
|
||||
if (state.state_machine == SYSTEM_STATE_MANUAL ||
|
||||
state.state_machine == SYSTEM_STATE_GROUND_READY ||
|
||||
state.state_machine == SYSTEM_STATE_STABILIZED ||
|
||||
state.state_machine == SYSTEM_STATE_AUTO ||
|
||||
state.state_machine == SYSTEM_STATE_MISSION_ABORT ||
|
||||
state.state_machine == SYSTEM_STATE_EMCY_LANDING) {
|
||||
if (state.flag_system_armed) {
|
||||
att_sp.thrust = manual.throttle;
|
||||
armed.armed = true;
|
||||
|
||||
@@ -173,7 +166,7 @@ mc_thread_main(int argc, char *argv[])
|
||||
|
||||
}
|
||||
|
||||
multirotor_control_attitude(&att_sp, &att, &state, &actuator_controls, motor_test_mode);
|
||||
multirotor_control_attitude(&att_sp, &att, &state, &actuators, motor_test_mode);
|
||||
|
||||
/* publish the result */
|
||||
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
|
||||
|
||||
Reference in New Issue
Block a user