Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 17:35:22 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

added generic class for vtol types

Browse Source
This commit is contained in:
tumbili
2015-06-18 23:55:30 +02:00
parent 2485e03794
commit 1ccded0305
2 changed files with 248 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vtol_att_control/vtol_type.cpp
+133
View File
@@ -0,0 +1,133 @@
/****************************************************************************
*
* Copyright (c) 2015 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.
*
****************************************************************************/
/**
* @file airframe.cpp
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#include "vtol_type.h"
#include "drivers/drv_pwm_output.h"
#include <nuttx/fs/ioctl.h>
#include "vtol_att_control_main.h"
VtolType::VtolType(VtolAttitudeControl *att_controller) :
_attc(att_controller),
_vtol_mode(ROTARY_WING)
{
_v_att = _attc->get_att();
_v_att_sp = _attc->get_att_sp();
_v_rates_sp = _attc->get_rates_sp();
_mc_virtual_v_rates_sp = _attc->get_mc_virtual_rates_sp();
_fw_virtual_v_rates_sp = _attc->get_fw_virtual_rates_sp();
_manual_control_sp = _attc->get_manual_control_sp();
_v_control_mode = _attc->get_control_mode();
_vtol_vehicle_status = _attc->get_vehicle_status();
_actuators_out_0 = _attc->get_actuators_out0();
_actuators_out_1 = _attc->get_actuators_out1();
_actuators_mc_in = _attc->get_actuators_mc_in();
_actuators_fw_in = _attc->get_actuators_fw_in();
_armed = _attc->get_armed();
_local_pos = _attc->get_local_pos();
_airspeed = _attc->get_airspeed();
_batt_status = _attc->get_batt_status();
_params = _attc->get_params();
flag_idle_mc = true;
}
VtolType::~VtolType()
{
}
/**
* Adjust idle speed for mc mode.
*/
void VtolType::set_idle_mc()
{
int ret;
unsigned servo_count;
char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = open(dev, 0);
if (fd < 0) {err(1, "can't open %s", dev);}
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
unsigned pwm_value = _params->idle_pwm_mc;
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
for (int i = 0; i < _params->vtol_motor_count; i++) {
pwm_values.values[i] = pwm_value;
pwm_values.channel_count++;
}
ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {errx(ret, "failed setting min values");}
close(fd);
flag_idle_mc = true;
}
/**
* Adjust idle speed for fw mode.
*/
void VtolType::set_idle_fw()
{
int ret;
char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = open(dev, 0);
if (fd < 0) {err(1, "can't open %s", dev);}
unsigned pwm_value = PWM_LOWEST_MIN;
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
for (int i = 0; i < _params->vtol_motor_count; i++) {
pwm_values.values[i] = pwm_value;
pwm_values.channel_count++;
}
ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {errx(ret, "failed setting min values");}
close(fd);
}
src/modules/vtol_att_control/vtol_type.h
+115
View File
@@ -0,0 +1,115 @@
/****************************************************************************
*
* Copyright (c) 2015 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.
*
****************************************************************************/
/**
* @file airframe.h
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#ifndef VTOL_YYPE_H
#define VTOL_YYPE_H
struct Params {
int idle_pwm_mc; // pwm value for idle in mc mode
int vtol_motor_count; // number of motors
int vtol_fw_permanent_stab; // in fw mode stabilize attitude also in manual mode
float mc_airspeed_min; // min airspeed in multicoper mode (including prop-wash)
float mc_airspeed_trim; // trim airspeed in multicopter mode
float mc_airspeed_max; // max airpseed in multicopter mode
float fw_pitch_trim; // trim for neutral elevon position in fw mode
float power_max; // maximum power of one engine
float prop_eff; // factor to calculate prop efficiency
float arsp_lp_gain; // total airspeed estimate low pass gain
int vtol_type;
};
enum mode {
ROTARY_WING = 0,
FIXED_WING,
TRANSITION,
EXTERNAL
};
class VtolAttitudeControl;
class VtolType
{
public:
VtolType(VtolAttitudeControl *att_controller);
virtual ~VtolType();
virtual void update_vtol_state() = 0;
virtual void update_mc_state() = 0;
virtual void process_mc_data() = 0;
virtual void update_fw_state() = 0;
virtual void process_fw_data() = 0;
virtual void update_transition_state() = 0;
virtual void update_external_state() = 0;
void set_idle_mc();
void set_idle_fw();
mode get_mode () {return _vtol_mode;};
protected:
VtolAttitudeControl *_attc;
mode _vtol_mode;
struct vehicle_attitude_s *_v_att; //vehicle attitude
struct vehicle_attitude_setpoint_s *_v_att_sp; //vehicle attitude setpoint
struct vehicle_rates_setpoint_s *_v_rates_sp; //vehicle rates setpoint
struct vehicle_rates_setpoint_s *_mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint
struct vehicle_rates_setpoint_s *_fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint
struct manual_control_setpoint_s *_manual_control_sp; //manual control setpoint
struct vehicle_control_mode_s *_v_control_mode; //vehicle control mode
struct vtol_vehicle_status_s *_vtol_vehicle_status;
struct actuator_controls_s *_actuators_out_0; //actuator controls going to the mc mixer
struct actuator_controls_s *_actuators_out_1; //actuator controls going to the fw mixer (used for elevons)
struct actuator_controls_s *_actuators_mc_in; //actuator controls from mc_att_control
struct actuator_controls_s *_actuators_fw_in; //actuator controls from fw_att_control
struct actuator_armed_s *_armed; //actuator arming status
struct vehicle_local_position_s *_local_pos;
struct airspeed_s *_airspeed; // airspeed
struct battery_status_s *_batt_status; // battery status
struct Params *_params;
bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode"
};
#endif