fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-06 17:00:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into ekf_acc_comp

Browse Source
This commit is contained in:
Anton Babushkin
2013-11-02 18:30:03 +04:00
parent e413ae7cbc 2fd1aed6be
commit 34276c17ca
36 changed files with 1421 additions and 721 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+12 -6
View File
@@ -199,7 +199,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode
*/
int commander_thread_main(int argc, char *argv[]);
void control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed);
void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed);
void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
@@ -843,6 +843,12 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(safety), safety_sub, &safety);
// XXX this would be the right approach to do it, but do we *WANT* this?
// /* disarm if safety is now on and still armed */
// if (safety.safety_switch_available && !safety.safety_off) {
// (void)arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
// }
}
/* update global position estimate */
@@ -1219,7 +1225,7 @@ int commander_thread_main(int argc, char *argv[])
}
/* play arming and battery warning tunes */
if (!arm_tune_played && armed.armed) {
if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) {
/* play tune when armed */
if (tune_arm() == OK)
arm_tune_played = true;
@@ -1240,7 +1246,7 @@ int commander_thread_main(int argc, char *argv[])
}
/* reset arm_tune_played when disarmed */
if (!(armed.armed && (!safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) {
if (status.arming_state != ARMING_STATE_ARMED || (safety.safety_switch_available && !safety.safety_off)) {
arm_tune_played = false;
}
@@ -1309,7 +1315,7 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val
}
void
control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed)
control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed)
{
/* driving rgbled */
if (changed) {
@@ -1356,11 +1362,11 @@ control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chan
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
/* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */
if (armed->armed) {
if (actuator_armed->armed) {
/* armed, solid */
led_on(LED_BLUE);
} else if (armed->ready_to_arm) {
} else if (actuator_armed->ready_to_arm) {
/* ready to arm, blink at 1Hz */
if (leds_counter % 20 == 0)
led_toggle(LED_BLUE);
src/modules/mavlink/missionlib.c
+1 -1
View File
@@ -220,7 +220,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
sp.lon = param6_lon_y * 1e7f;
sp.altitude = param7_alt_z;
sp.altitude_is_relative = false;
sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
set_special_fields(param1, param2, param3, param4, command, &sp);
/* Initialize setpoint publication if necessary */
src/modules/px4iofirmware/mixer.cpp
+31 -114
View File
@@ -47,6 +47,7 @@
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h>
#include <systemlib/pwm_limit/pwm_limit.h>
#include <systemlib/mixer/mixer.h>
extern "C" {
@@ -59,12 +60,6 @@ extern "C" {
*/
#define FMU_INPUT_DROP_LIMIT_US 200000
/*
* Time that the ESCs need to initialize
*/
#define ESC_INIT_TIME_US 1000000
#define ESC_RAMP_TIME_US 2000000
/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
#define ROLL 0
#define PITCH 1
@@ -76,15 +71,6 @@ extern "C" {
static bool mixer_servos_armed = false;
static bool should_arm = false;
static bool should_always_enable_pwm = false;
static uint64_t esc_init_time;
enum esc_state_e {
ESC_OFF,
ESC_INIT,
ESC_RAMP,
ESC_ON
};
static esc_state_e esc_state;
/* selected control values and count for mixing */
enum mixer_source {
@@ -165,102 +151,6 @@ mixer_tick(void)
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE);
}
/*
* Run the mixers.
*/
if (source == MIX_FAILSAFE) {
/* copy failsafe values to the servo outputs */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_servos[i] = r_page_servo_failsafe[i];
/* safe actuators for FMU feedback */
r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f;
}
} else if (source != MIX_NONE) {
float outputs[PX4IO_SERVO_COUNT];
unsigned mixed;
uint16_t ramp_promille;
/* update esc init state, but only if we are truely armed and not just PWM enabled */
if (mixer_servos_armed && should_arm) {
switch (esc_state) {
/* after arming, some ESCs need an initalization period, count the time from here */
case ESC_OFF:
esc_init_time = hrt_absolute_time();
esc_state = ESC_INIT;
break;
/* after waiting long enough for the ESC initialization, we can start with the ramp to start the ESCs */
case ESC_INIT:
if (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US) {
esc_state = ESC_RAMP;
}
break;
/* then ramp until the min speed is reached */
case ESC_RAMP:
if (hrt_elapsed_time(&esc_init_time) > (ESC_INIT_TIME_US + ESC_RAMP_TIME_US)) {
esc_state = ESC_ON;
}
break;
case ESC_ON:
default:
break;
}
} else {
esc_state = ESC_OFF;
}
/* do the calculations during the ramp for all at once */
if (esc_state == ESC_RAMP) {
ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US;
}
/* mix */
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
/* scale to PWM and update the servo outputs as required */
for (unsigned i = 0; i < mixed; i++) {
/* save actuator values for FMU readback */
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
switch (esc_state) {
case ESC_INIT:
r_page_servos[i] = (outputs[i] * 600 + 1500);
break;
case ESC_RAMP:
r_page_servos[i] = (outputs[i]
* (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 - ramp_promille*r_page_servo_control_min[i] - (1000-ramp_promille)*900)/2/1000
+ (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 + ramp_promille*r_page_servo_control_min[i] + (1000-ramp_promille)*900)/2/1000);
break;
case ESC_ON:
r_page_servos[i] = (outputs[i]
* (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2
+ (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2);
break;
case ESC_OFF:
default:
break;
}
}
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
r_page_servos[i] = 0;
}
/*
* Decide whether the servos should be armed right now.
*
@@ -285,6 +175,34 @@ mixer_tick(void)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
/*
* Run the mixers.
*/
if (source == MIX_FAILSAFE) {
/* copy failsafe values to the servo outputs */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_servos[i] = r_page_servo_failsafe[i];
/* safe actuators for FMU feedback */
r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f;
}
} else if (source != MIX_NONE) {
float outputs[PX4IO_SERVO_COUNT];
unsigned mixed;
/* mix */
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
r_page_servos[i] = 0;
}
if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
/* need to arm, but not armed */
up_pwm_servo_arm(true);
@@ -298,7 +216,6 @@ mixer_tick(void)
mixer_servos_armed = false;
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
isr_debug(5, "> PWM disabled");
}
if (mixer_servos_armed && should_arm) {
@@ -307,9 +224,9 @@ mixer_tick(void)
up_pwm_servo_set(i, r_page_servos[i]);
} else if (mixer_servos_armed && should_always_enable_pwm) {
/* set the idle servo outputs. */
/* set the disarmed servo outputs. */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
up_pwm_servo_set(i, r_page_servo_idle[i]);
up_pwm_servo_set(i, r_page_servo_disarmed[i]);
}
}
src/modules/px4iofirmware/module.mk
+1
View File
@@ -14,6 +14,7 @@ SRCS = adc.c \
../systemlib/mixer/mixer_group.cpp \
../systemlib/mixer/mixer_multirotor.cpp \
../systemlib/mixer/mixer_simple.cpp \
../systemlib/pwm_limit/pwm_limit.c
ifeq ($(BOARD),px4io-v1)
SRCS += i2c.c
src/modules/px4iofirmware/protocol.h
+2 -2
View File
@@ -220,8 +220,8 @@ enum { /* DSM bind states */
/* PWM maximum values for certain ESCs */
#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM idle values that are active, even when SAFETY_SAFE */
#define PX4IO_PAGE_IDLE_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM disarmed values that are active, even when SAFETY_SAFE */
#define PX4IO_PAGE_DISARMED_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/**
* As-needed mixer data upload.
src/modules/px4iofirmware/px4io.c
+6
View File
@@ -50,6 +50,7 @@
#include <drivers/drv_hrt.h>
#include <systemlib/perf_counter.h>
#include <systemlib/pwm_limit/pwm_limit.h>
#include <stm32_uart.h>
@@ -64,6 +65,8 @@ struct sys_state_s system_state;
static struct hrt_call serial_dma_call;
pwm_limit_t pwm_limit;
/*
* a set of debug buffers to allow us to send debug information from ISRs
*/
@@ -171,6 +174,9 @@ user_start(int argc, char *argv[])
struct mallinfo minfo = mallinfo();
lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);
/* initialize PWM limit lib */
pwm_limit_init(&pwm_limit);
#if 0
/* not enough memory, lock down */
if (minfo.mxordblk < 500) {
src/modules/px4iofirmware/px4io.h
+8 -1
View File
@@ -46,6 +46,8 @@
#include "protocol.h"
#include <systemlib/pwm_limit/pwm_limit.h>
/*
* Constants and limits.
*/
@@ -80,7 +82,7 @@ extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */
extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */
extern uint16_t r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */
extern uint16_t r_page_servo_idle[]; /* PX4IO_PAGE_IDLE_PWM */
extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
/*
* Register aliases.
@@ -122,6 +124,11 @@ struct sys_state_s {
extern struct sys_state_s system_state;
/*
* PWM limit structure
*/
extern pwm_limit_t pwm_limit;
/*
* GPIO handling.
*/
src/modules/px4iofirmware/registers.c
+62 -45
View File
@@ -199,7 +199,7 @@ uint16_t r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRI
*
* Disable pulses as default.
*/
uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 };
uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 };
/**
* PAGE 106
@@ -207,7 +207,7 @@ uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 };
* minimum PWM values when armed
*
*/
uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN };
/**
* PAGE 107
@@ -215,15 +215,15 @@ uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 90
* maximum PWM values when armed
*
*/
uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 };
uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX };
/**
* PAGE 108
*
* idle PWM values for difficult ESCs
* disarmed PWM values for difficult ESCs
*
*/
uint16_t r_page_servo_idle[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
uint16_t r_page_servo_disarmed[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 };
int
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
@@ -276,8 +276,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
/* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
/* XXX range-check value? */
r_page_servo_failsafe[offset] = *values;
if (*values == 0) {
/* ignore 0 */
} else if (*values < PWM_LOWEST_MIN) {
r_page_servo_failsafe[offset] = PWM_LOWEST_MIN;
} else if (*values > PWM_HIGHEST_MAX) {
r_page_servo_failsafe[offset] = PWM_HIGHEST_MAX;
} else {
r_page_servo_failsafe[offset] = *values;
}
/* flag the failsafe values as custom */
r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM;
@@ -293,16 +300,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
/* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
if (*values == 0)
/* set to default */
r_page_servo_control_min[offset] = 900;
else if (*values > 1200)
r_page_servo_control_min[offset] = 1200;
else if (*values < 900)
r_page_servo_control_min[offset] = 900;
else
if (*values == 0) {
/* ignore 0 */
} else if (*values > PWM_HIGHEST_MIN) {
r_page_servo_control_min[offset] = PWM_HIGHEST_MIN;
} else if (*values < PWM_LOWEST_MIN) {
r_page_servo_control_min[offset] = PWM_LOWEST_MIN;
} else {
r_page_servo_control_min[offset] = *values;
}
offset++;
num_values--;
@@ -315,16 +321,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
/* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
if (*values == 0)
/* set to default */
r_page_servo_control_max[offset] = 2100;
else if (*values > 2100)
r_page_servo_control_max[offset] = 2100;
else if (*values < 1800)
r_page_servo_control_max[offset] = 1800;
else
if (*values == 0) {
/* ignore 0 */
} else if (*values > PWM_HIGHEST_MAX) {
r_page_servo_control_max[offset] = PWM_HIGHEST_MAX;
} else if (*values < PWM_LOWEST_MAX) {
r_page_servo_control_max[offset] = PWM_LOWEST_MAX;
} else {
r_page_servo_control_max[offset] = *values;
}
offset++;
num_values--;
@@ -332,28 +337,40 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
}
break;
case PX4IO_PAGE_IDLE_PWM:
case PX4IO_PAGE_DISARMED_PWM:
{
/* flag for all outputs */
bool all_disarmed_off = true;
/* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
/* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
if (*values == 0)
/* set to default */
r_page_servo_idle[offset] = 0;
if (*values == 0) {
/* 0 means disabling always PWM */
r_page_servo_disarmed[offset] = 0;
} else if (*values < PWM_LOWEST_MIN) {
r_page_servo_disarmed[offset] = PWM_LOWEST_MIN;
all_disarmed_off = false;
} else if (*values > PWM_HIGHEST_MAX) {
r_page_servo_disarmed[offset] = PWM_HIGHEST_MAX;
all_disarmed_off = false;
} else {
r_page_servo_disarmed[offset] = *values;
all_disarmed_off = false;
}
else if (*values < 900)
r_page_servo_idle[offset] = 900;
else if (*values > 2100)
r_page_servo_idle[offset] = 2100;
else
r_page_servo_idle[offset] = *values;
offset++;
num_values--;
values++;
}
/* flag the failsafe values as custom */
r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE;
offset++;
num_values--;
values++;
if (all_disarmed_off) {
/* disable PWM output if disarmed */
r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE);
} else {
/* enable PWM output always */
r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE;
}
}
break;
@@ -767,8 +784,8 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
case PX4IO_PAGE_CONTROL_MAX_PWM:
SELECT_PAGE(r_page_servo_control_max);
break;
case PX4IO_PAGE_IDLE_PWM:
SELECT_PAGE(r_page_servo_idle);
case PX4IO_PAGE_DISARMED_PWM:
SELECT_PAGE(r_page_servo_disarmed);
break;
default:
src/modules/systemlib/mixer/mixer.h
+1
View File
@@ -449,6 +449,7 @@ public:
HEX_PLUS, /**< hex in + configuration */
OCTA_X,
OCTA_PLUS,
OCTA_COX,
MAX_GEOMETRY
};
src/modules/systemlib/mixer/mixer_multirotor.cpp
+15
View File
@@ -130,6 +130,16 @@ const MultirotorMixer::Rotor _config_octa_plus[] = {
{ 1.000000, 0.000000, -1.00 },
{ -1.000000, 0.000000, -1.00 },
};
const MultirotorMixer::Rotor _config_octa_cox[] = {
{ -0.707107, 0.707107, 1.00 },
{ 0.707107, 0.707107, -1.00 },
{ 0.707107, -0.707107, 1.00 },
{ -0.707107, -0.707107, -1.00 },
{ 0.707107, 0.707107, 1.00 },
{ -0.707107, 0.707107, -1.00 },
{ -0.707107, -0.707107, 1.00 },
{ 0.707107, -0.707107, -1.00 },
};
const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = {
&_config_quad_x[0],
&_config_quad_plus[0],
@@ -139,6 +149,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = {
&_config_hex_plus[0],
&_config_octa_x[0],
&_config_octa_plus[0],
&_config_octa_cox[0],
};
const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = {
4, /* quad_x */
@@ -149,6 +160,7 @@ const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = {
6, /* hex_plus */
8, /* octa_x */
8, /* octa_plus */
8, /* octa_cox */
};
}
@@ -240,6 +252,9 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
} else if (!strcmp(geomname, "8x")) {
geometry = MultirotorMixer::OCTA_X;
} else if (!strcmp(geomname, "8c")) {
geometry = MultirotorMixer::OCTA_COX;
} else {
debug("unrecognised geometry '%s'", geomname);
src/modules/systemlib/mixer/multi_tables
+12 -1
View File
@@ -74,7 +74,18 @@ set octa_plus {
90 CW
}
set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus octa_x octa_plus}
set octa_cox {
45 CCW
-45 CW
-135 CCW
135 CW
-45 CCW
45 CW
135 CCW
-135 CW
}
set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus octa_x octa_plus octa_cox}
proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]}
src/modules/systemlib/pwm_limit/pwm_limit.c
+133
View File
@@ -0,0 +1,133 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Julian Oes <joes@student.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 pwm_limit.c
*
* Lib to limit PWM output
*
* @author Julian Oes <joes@student.ethz.ch>
*/
#include "pwm_limit.h"
#include <math.h>
#include <stdbool.h>
#include <drivers/drv_hrt.h>
void pwm_limit_init(pwm_limit_t *limit)
{
limit->state = LIMIT_STATE_OFF;
limit->time_armed = 0;
return;
}
void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit)
{
/* first evaluate state changes */
switch (limit->state) {
case LIMIT_STATE_OFF:
if (armed)
limit->state = LIMIT_STATE_RAMP;
limit->time_armed = hrt_absolute_time();
break;
case LIMIT_STATE_INIT:
if (!armed)
limit->state = LIMIT_STATE_OFF;
else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US)
limit->state = LIMIT_STATE_RAMP;
break;
case LIMIT_STATE_RAMP:
if (!armed)
limit->state = LIMIT_STATE_OFF;
else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US + RAMP_TIME_US)
limit->state = LIMIT_STATE_ON;
break;
case LIMIT_STATE_ON:
if (!armed)
limit->state = LIMIT_STATE_OFF;
break;
default:
break;
}
unsigned progress;
uint16_t temp_pwm;
/* then set effective_pwm based on state */
switch (limit->state) {
case LIMIT_STATE_OFF:
case LIMIT_STATE_INIT:
for (unsigned i=0; i<num_channels; i++) {
effective_pwm[i] = disarmed_pwm[i];
output[i] = 0.0f;
}
break;
case LIMIT_STATE_RAMP:
progress = (hrt_absolute_time() - INIT_TIME_US - limit->time_armed)*10000 / RAMP_TIME_US;
for (unsigned i=0; i<num_channels; i++) {
uint16_t ramp_min_pwm;
/* if a disarmed pwm value was set, blend between disarmed and min */
if (disarmed_pwm[i] > 0) {
/* safeguard against overflows */
uint16_t disarmed = disarmed_pwm[i];
if (disarmed > min_pwm[i])
disarmed = min_pwm[i];
uint16_t disarmed_min_diff = min_pwm[i] - disarmed;
ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000;
} else {
/* no disarmed pwm value set, choose min pwm */
ramp_min_pwm = min_pwm[i];
}
effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2;
output[i] = (float)progress/10000.0f * output[i];
}
break;
case LIMIT_STATE_ON:
for (unsigned i=0; i<num_channels; i++) {
effective_pwm[i] = output[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2;
/* effective_output stays the same */
}
break;
default:
break;
}
return;
}
src/modules/systemlib/pwm_limit/pwm_limit.h
+77
View File
@@ -0,0 +1,77 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Julian Oes <joes@student.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 pwm_limit.h
*
* Lib to limit PWM output
*
* @author Julian Oes <joes@student.ethz.ch>
*/
#ifndef PWM_LIMIT_H_
#define PWM_LIMIT_H_
#include <stdint.h>
#include <stdbool.h>
/*
* time for the ESCs to initialize
* (this is not actually needed if PWM is sent right after boot)
*/
#define INIT_TIME_US 500000
/*
* time to slowly ramp up the ESCs
*/
#define RAMP_TIME_US 2500000
typedef struct {
enum {
LIMIT_STATE_OFF = 0,
LIMIT_STATE_INIT,
LIMIT_STATE_RAMP,
LIMIT_STATE_ON
} state;
uint64_t time_armed;
} pwm_limit_t;
__BEGIN_DECLS
__EXPORT void pwm_limit_init(pwm_limit_t *limit);
__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit);
__END_DECLS
#endif /* PWM_LIMIT_H_ */
src/modules/uORB/topics/actuator_outputs.h
+1 -1
View File
@@ -60,7 +60,7 @@
struct actuator_outputs_s {
uint64_t timestamp; /**< output timestamp in us since system boot */
float output[NUM_ACTUATOR_OUTPUTS]; /**< output data, in natural output units */
int noutputs; /**< valid outputs */
unsigned noutputs; /**< valid outputs */
};
/**