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

merge master

Browse Source
This commit is contained in:
Lorenz Meier
2014-01-31 09:51:59 +01:00
parent fb446c01b8 83df116c7a
commit d2b183c05b
121 changed files with 4043 additions and 3593 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+66 -56
View File
@@ -107,14 +107,9 @@ static const int ERROR = -1;
extern struct system_load_s system_load;
#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
/* Decouple update interval and hysteris counters, all depends on intervals */
#define COMMANDER_MONITORING_INTERVAL 50000
#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))
#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
#define MAVLINK_OPEN_INTERVAL 50000
@@ -159,6 +154,16 @@ static bool on_usb_power = false;
static float takeoff_alt = 5.0f;
static struct vehicle_status_s status;
/* armed topic */
static struct actuator_armed_s armed;
static struct safety_s safety;
/* flags for control apps */
struct vehicle_control_mode_s control_mode;
/* tasks waiting for low prio thread */
typedef enum {
LOW_PRIO_TASK_NONE = 0,
@@ -215,6 +220,9 @@ void print_reject_arm(const char *msg);
void print_status();
int arm();
int disarm();
transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
/**
@@ -282,6 +290,16 @@ int commander_main(int argc, char *argv[])
exit(0);
}
if (!strcmp(argv[1], "arm")) {
arm();
exit(0);
}
if (!strcmp(argv[1], "disarm")) {
disarm();
exit(0);
}
usage("unrecognized command");
exit(1);
}
@@ -349,6 +367,30 @@ void print_status()
static orb_advert_t control_mode_pub;
static orb_advert_t status_pub;
int arm()
{
int arming_res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed);
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
return 0;
} else {
return 1;
}
}
int disarm()
{
int arming_res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
return 0;
} else {
return 1;
}
}
void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
{
/* result of the command */
@@ -542,16 +584,6 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
static struct vehicle_status_s status;
/* armed topic */
static struct actuator_armed_s armed;
static struct safety_s safety;
/* flags for control apps */
struct vehicle_control_mode_s control_mode;
int commander_thread_main(int argc, char *argv[])
{
/* not yet initialized */
@@ -666,8 +698,6 @@ int commander_thread_main(int argc, char *argv[])
/* Start monitoring loop */
unsigned counter = 0;
unsigned low_voltage_counter = 0;
unsigned critical_voltage_counter = 0;
unsigned stick_off_counter = 0;
unsigned stick_on_counter = 0;
@@ -745,7 +775,6 @@ int commander_thread_main(int argc, char *argv[])
int battery_sub = orb_subscribe(ORB_ID(battery_status));
struct battery_status_s battery;
memset(&battery, 0, sizeof(battery));
battery.voltage_v = 0.0f;
/* Subscribe to subsystem info topic */
int subsys_sub = orb_subscribe(ORB_ID(subsystem_info));
@@ -890,14 +919,12 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
// warnx("bat v: %2.2f", battery.voltage_v);
/* only consider battery voltage if system has been running 2s and battery voltage is higher than 4V */
if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 4.0f) {
status.battery_voltage = battery.voltage_v;
/* only consider battery voltage if system has been running 2s and battery voltage is valid */
if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
status.battery_voltage = battery.voltage_filtered_v;
status.battery_current = battery.current_a;
status.condition_battery_voltage_valid = true;
status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage);
status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah);
}
}
@@ -950,46 +977,29 @@ int commander_thread_main(int argc, char *argv[])
//on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
}
// XXX remove later
//warnx("bat remaining: %2.2f", status.battery_remaining);
/* if battery voltage is getting lower, warn using buzzer, etc. */
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
//TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
low_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
battery_tune_played = false;
}
low_voltage_counter++;
low_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
battery_tune_played = false;
} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
/* critical battery voltage, this is rather an emergency, change state machine */
if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
critical_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
battery_tune_played = false;
critical_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
battery_tune_played = false;
if (armed.armed) {
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed);
if (armed.armed) {
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed);
} else {
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed);
}
status_changed = true;
} else {
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed);
}
critical_voltage_counter++;
} else {
low_voltage_counter = 0;
critical_voltage_counter = 0;
status_changed = true;
}
/* End battery voltage check */
src/modules/commander/commander_helper.cpp
+28 -16
View File
@@ -44,6 +44,7 @@
#include <stdint.h>
#include <stdbool.h>
#include <fcntl.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
@@ -251,36 +252,47 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern)
ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
}
float battery_remaining_estimate_voltage(float voltage)
float battery_remaining_estimate_voltage(float voltage, float discharged)
{
float ret = 0;
static param_t bat_volt_empty;
static param_t bat_volt_full;
static param_t bat_n_cells;
static param_t bat_v_empty_h;
static param_t bat_v_full_h;
static param_t bat_n_cells_h;
static param_t bat_capacity_h;
static float bat_v_empty = 3.2f;
static float bat_v_full = 4.0f;
static int bat_n_cells = 3;
static float bat_capacity = -1.0f;
static bool initialized = false;
static unsigned int counter = 0;
static float ncells = 3;
// XXX change cells to int (and param to INT32)
if (!initialized) {
bat_volt_empty = param_find("BAT_V_EMPTY");
bat_volt_full = param_find("BAT_V_FULL");
bat_n_cells = param_find("BAT_N_CELLS");
bat_v_empty_h = param_find("BAT_V_EMPTY");
bat_v_full_h = param_find("BAT_V_FULL");
bat_n_cells_h = param_find("BAT_N_CELLS");
bat_capacity_h = param_find("BAT_CAPACITY");
initialized = true;
}
static float chemistry_voltage_empty = 3.2f;
static float chemistry_voltage_full = 4.05f;
if (counter % 100 == 0) {
param_get(bat_volt_empty, &chemistry_voltage_empty);
param_get(bat_volt_full, &chemistry_voltage_full);
param_get(bat_n_cells, &ncells);
param_get(bat_v_empty_h, &bat_v_empty);
param_get(bat_v_full_h, &bat_v_full);
param_get(bat_n_cells_h, &bat_n_cells);
param_get(bat_capacity_h, &bat_capacity);
}
counter++;
ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty));
/* remaining charge estimate based on voltage */
float remaining_voltage = (voltage - bat_n_cells * bat_v_empty) / (bat_n_cells * (bat_v_full - bat_v_empty));
if (bat_capacity > 0.0f) {
/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */
ret = fminf(remaining_voltage, 1.0f - discharged / bat_capacity);
} else {
/* else use voltage */
ret = remaining_voltage;
}
/* limit to sane values */
ret = (ret < 0.0f) ? 0.0f : ret;
src/modules/commander/commander_helper.h
+4 -3
View File
@@ -75,12 +75,13 @@ void rgbled_set_mode(rgbled_mode_t mode);
void rgbled_set_pattern(rgbled_pattern_t *pattern);
/**
* Provides a coarse estimate of remaining battery power.
* Estimate remaining battery charge.
*
* The estimate is very basic and based on decharging voltage curves.
* Use integral of current if battery capacity known (BAT_CAPACITY parameter set),
* else use simple estimate based on voltage.
*
* @return the estimated remaining capacity in 0..1
*/
float battery_remaining_estimate_voltage(float voltage);
float battery_remaining_estimate_voltage(float voltage, float discharged);
#endif /* COMMANDER_HELPER_H_ */
src/modules/commander/commander_params.c
+4 -3
View File
@@ -50,6 +50,7 @@ PARAM_DEFINE_FLOAT(NAV_TAKEOFF_GAP, 3.0f);
PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f);
PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f);
PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3);
PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f);
PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
src/modules/mavlink/mavlink.c
+2 -2
View File
@@ -677,8 +677,8 @@ int mavlink_thread_main(int argc, char *argv[])
v_status.onboard_control_sensors_health,
v_status.load * 1000.0f,
v_status.battery_voltage * 1000.0f,
v_status.battery_current * 1000.0f,
v_status.battery_remaining,
v_status.battery_current * 100.0f,
v_status.battery_remaining * 100.0f,
v_status.drop_rate_comm,
v_status.errors_comm,
v_status.errors_count1,
src/modules/mavlink/orb_listener.c
+2 -2
View File
@@ -352,10 +352,10 @@ l_input_rc(const struct listener *l)
const unsigned port_width = 8;
for (unsigned i = 0; (i * port_width) < (rc_raw.channel_count + port_width); i++) {
for (unsigned i = 0; (i * port_width) < rc_raw.channel_count; i++) {
/* Channels are sent in MAVLink main loop at a fixed interval */
mavlink_msg_rc_channels_raw_send(chan,
rc_raw.timestamp / 1000,
rc_raw.timestamp_publication / 1000,
i,
(rc_raw.channel_count > (i * port_width) + 0) ? rc_raw.values[(i * port_width) + 0] : UINT16_MAX,
(rc_raw.channel_count > (i * port_width) + 1) ? rc_raw.values[(i * port_width) + 1] : UINT16_MAX,
src/modules/px4iofirmware/adc.c
+23 -30
View File
@@ -83,6 +83,14 @@ adc_init(void)
{
adc_perf = perf_alloc(PC_ELAPSED, "adc");
/* put the ADC into power-down mode */
rCR2 &= ~ADC_CR2_ADON;
up_udelay(10);
/* bring the ADC out of power-down mode */
rCR2 |= ADC_CR2_ADON;
up_udelay(10);
/* do calibration if supported */
#ifdef ADC_CR2_CAL
rCR2 |= ADC_CR2_RSTCAL;
@@ -96,41 +104,25 @@ adc_init(void)
if (rCR2 & ADC_CR2_CAL)
return -1;
#endif
/* arbitrarily configure all channels for 55 cycle sample time */
rSMPR1 = 0b00000011011011011011011011011011;
/*
* Configure sampling time.
*
* For electrical protection reasons, we want to be able to have
* 10K in series with ADC inputs that leave the board. At 12MHz this
* means we need 28.5 cycles of sampling time (per table 43 in the
* datasheet).
*/
rSMPR1 = 0b00000000011011011011011011011011;
rSMPR2 = 0b00011011011011011011011011011011;
/* XXX for F2/4, might want to select 12-bit mode? */
rCR1 = 0;
/* enable the temperature sensor / Vrefint channel if supported*/
rCR2 =
#ifdef ADC_CR2_TSVREFE
/* enable the temperature sensor in CR2 */
ADC_CR2_TSVREFE |
#endif
0;
#ifdef ADC_CCR_TSVREFE
/* enable temperature sensor in CCR */
rCCR = ADC_CCR_TSVREFE;
#endif
rCR2 |= ADC_CR2_TSVREFE; /* enable the temperature sensor / Vrefint channel */
/* configure for a single-channel sequence */
rSQR1 = 0;
rSQR2 = 0;
rSQR3 = 0; /* will be updated with the channel each tick */
/* power-cycle the ADC and turn it on */
rCR2 &= ~ADC_CR2_ADON;
up_udelay(10);
rCR2 |= ADC_CR2_ADON;
up_udelay(10);
rCR2 |= ADC_CR2_ADON;
up_udelay(10);
rSQR3 = 0; /* will be updated with the channel at conversion time */
return 0;
}
@@ -141,11 +133,12 @@ adc_init(void)
uint16_t
adc_measure(unsigned channel)
{
perf_begin(adc_perf);
/* clear any previous EOC */
if (rSR & ADC_SR_EOC)
rSR &= ~ADC_SR_EOC;
rSR = 0;
(void)rDR;
/* run a single conversion right now - should take about 60 cycles (a few microseconds) max */
rSQR3 = channel;
@@ -158,7 +151,6 @@ adc_measure(unsigned channel)
/* never spin forever - this will give a bogus result though */
if (hrt_elapsed_time(&now) > 100) {
debug("adc timeout");
perf_end(adc_perf);
return 0xffff;
}
@@ -166,6 +158,7 @@ adc_measure(unsigned channel)
/* read the result and clear EOC */
uint16_t result = rDR;
rSR = 0;
perf_end(adc_perf);
return result;
src/modules/px4iofirmware/controls.c
+139 -83
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 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
@@ -59,6 +59,11 @@ static perf_counter_t c_gather_ppm;
void
controls_init(void)
{
/* no channels */
r_raw_rc_count = 0;
system_state.rc_channels_timestamp_received = 0;
system_state.rc_channels_timestamp_valid = 0;
/* DSM input (USART1) */
dsm_init("/dev/ttyS0");
@@ -97,26 +102,64 @@ controls_tick() {
/* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */
uint16_t rssi = 0;
#ifdef ADC_RSSI
if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) {
unsigned counts = adc_measure(ADC_RSSI);
if (counts != 0xffff) {
/* use 1:1 scaling on 3.3V ADC input */
unsigned mV = counts * 3300 / 4096;
/* scale to 0..253 */
rssi = mV / 13;
}
}
#endif
perf_begin(c_gather_dsm);
uint16_t temp_count = r_raw_rc_count;
bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
if (dsm_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
r_raw_rc_count = temp_count & 0x7fff;
if (temp_count & 0x8000)
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11;
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
else
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11;
r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
rssi = 255;
}
perf_end(c_gather_dsm);
perf_begin(c_gather_sbus);
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS);
bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS);
bool sbus_failsafe, sbus_frame_drop;
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS);
if (sbus_updated) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
rssi = 255;
if (sbus_frame_drop) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
rssi = 100;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
}
if (sbus_failsafe) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
rssi = 0;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
}
perf_end(c_gather_sbus);
/*
@@ -125,13 +168,12 @@ controls_tick() {
* disable the PPM decoder completely if we have S.bus signal.
*/
perf_begin(c_gather_ppm);
bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_status[PX4IO_P_STATUS_RC_DATA]);
bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);
if (ppm_updated) {
/* XXX sample RSSI properly here */
rssi = 255;
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
}
perf_end(c_gather_ppm);
@@ -139,6 +181,9 @@ controls_tick() {
if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
/* store RSSI */
r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;
/*
* In some cases we may have received a frame, but input has still
* been lost.
@@ -150,97 +195,100 @@ controls_tick() {
*/
if (dsm_updated || sbus_updated || ppm_updated) {
/* update RC-received timestamp */
system_state.rc_channels_timestamp = hrt_absolute_time();
/* record a bitmask of channels assigned */
unsigned assigned_channels = 0;
/* map raw inputs to mapped inputs */
/* XXX mapping should be atomic relative to protocol */
for (unsigned i = 0; i < r_raw_rc_count; i++) {
/* update RC-received timestamp */
system_state.rc_channels_timestamp_received = hrt_absolute_time();
/* map the input channel */
uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
/* do not command anything in failsafe, kick in the RC loss counter */
if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
/* update RC-received timestamp */
system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;
uint16_t raw = r_raw_rc_values[i];
/* map raw inputs to mapped inputs */
/* XXX mapping should be atomic relative to protocol */
for (unsigned i = 0; i < r_raw_rc_count; i++) {
int16_t scaled;
/* map the input channel */
uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
/*
* 1) Constrain to min/max values, as later processing depends on bounds.
*/
if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
raw = conf[PX4IO_P_RC_CONFIG_MIN];
if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
raw = conf[PX4IO_P_RC_CONFIG_MAX];
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
/*
* 2) Scale around the mid point differently for lower and upper range.
*
* This is necessary as they don't share the same endpoints and slope.
*
* First normalize to 0..1 range with correct sign (below or above center),
* then scale to 20000 range (if center is an actual center, -10000..10000,
* if parameters only support half range, scale to 10000 range, e.g. if
* center == min 0..10000, if center == max -10000..0).
*
* As the min and max bounds were enforced in step 1), division by zero
* cannot occur, as for the case of center == min or center == max the if
* statement is mutually exclusive with the arithmetic NaN case.
*
* DO NOT REMOVE OR ALTER STEP 1!
*/
if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
uint16_t raw = r_raw_rc_values[i];
} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
int16_t scaled;
} else {
/* in the configured dead zone, output zero */
scaled = 0;
}
/*
* 1) Constrain to min/max values, as later processing depends on bounds.
*/
if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
raw = conf[PX4IO_P_RC_CONFIG_MIN];
if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
raw = conf[PX4IO_P_RC_CONFIG_MAX];
/* invert channel if requested */
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
scaled = -scaled;
/*
* 2) Scale around the mid point differently for lower and upper range.
*
* This is necessary as they don't share the same endpoints and slope.
*
* First normalize to 0..1 range with correct sign (below or above center),
* then scale to 20000 range (if center is an actual center, -10000..10000,
* if parameters only support half range, scale to 10000 range, e.g. if
* center == min 0..10000, if center == max -10000..0).
*
* As the min and max bounds were enforced in step 1), division by zero
* cannot occur, as for the case of center == min or center == max the if
* statement is mutually exclusive with the arithmetic NaN case.
*
* DO NOT REMOVE OR ALTER STEP 1!
*/
if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
/* and update the scaled/mapped version */
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
if (mapped < PX4IO_CONTROL_CHANNELS) {
} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
/* invert channel if pitch - pulling the lever down means pitching up by convention */
if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
} else {
/* in the configured dead zone, output zero */
scaled = 0;
}
/* invert channel if requested */
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
scaled = -scaled;
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
assigned_channels |= (1 << mapped);
/* and update the scaled/mapped version */
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
if (mapped < PX4IO_CONTROL_CHANNELS) {
/* invert channel if pitch - pulling the lever down means pitching up by convention */
if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
scaled = -scaled;
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
assigned_channels |= (1 << mapped);
}
}
}
}
/* set un-assigned controls to zero */
for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
if (!(assigned_channels & (1 << i)))
r_rc_values[i] = 0;
}
/* set un-assigned controls to zero */
for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
if (!(assigned_channels & (1 << i)))
r_rc_values[i] = 0;
}
/*
* If we got an update with zero channels, treat it as
* a loss of input.
*
* This might happen if a protocol-based receiver returns an update
* that contains no channels that we have mapped.
*/
if (assigned_channels == 0 || rssi == 0) {
rc_input_lost = true;
} else {
/* set RC OK flag */
/* set RC OK flag, as we got an update */
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
/* if we have enough channels (5) to control the vehicle, the mapping is ok */
if (assigned_channels > 4) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
} else {
r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
}
}
/*
@@ -253,7 +301,7 @@ controls_tick() {
* If we haven't seen any new control data in 200ms, assume we
* have lost input.
*/
if (hrt_elapsed_time(&system_state.rc_channels_timestamp) > 200000) {
if (hrt_elapsed_time(&system_state.rc_channels_timestamp_received) > 200000) {
rc_input_lost = true;
/* clear the input-kind flags here */
@@ -261,24 +309,32 @@ controls_tick() {
PX4IO_P_STATUS_FLAGS_RC_PPM |
PX4IO_P_STATUS_FLAGS_RC_DSM |
PX4IO_P_STATUS_FLAGS_RC_SBUS);
}
/*
* Handle losing RC input
*/
if (rc_input_lost) {
/* this kicks in if the receiver is gone or the system went to failsafe */
if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
/* Clear the RC input status flag, clear manual override flag */
r_status_flags &= ~(
PX4IO_P_STATUS_FLAGS_OVERRIDE |
PX4IO_P_STATUS_FLAGS_RC_OK);
/* Mark all channels as invalid, as we just lost the RX */
r_rc_valid = 0;
/* Set the RC_LOST alarm */
r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
}
/* Mark the arrays as empty */
/* this kicks in if the receiver is completely gone */
if (rc_input_lost) {
/* Set channel count to zero */
r_raw_rc_count = 0;
r_rc_valid = 0;
}
/*
src/modules/px4iofirmware/mixer.cpp
+36 -9
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 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
@@ -71,6 +71,7 @@ extern "C" {
static bool mixer_servos_armed = false;
static bool should_arm = false;
static bool should_always_enable_pwm = false;
static volatile bool in_mixer = false;
/* selected control values and count for mixing */
enum mixer_source {
@@ -95,6 +96,7 @@ static void mixer_set_failsafe();
void
mixer_tick(void)
{
/* check that we are receiving fresh data from the FMU */
if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
@@ -199,13 +201,17 @@ mixer_tick(void)
}
} else if (source != MIX_NONE) {
} else if (source != MIX_NONE && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
float outputs[PX4IO_SERVO_COUNT];
unsigned mixed;
/* mix */
/* poor mans mutex */
in_mixer = true;
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
in_mixer = false;
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);
@@ -217,14 +223,25 @@ mixer_tick(void)
}
}
if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
/* set arming */
bool needs_to_arm = (should_arm || should_always_enable_pwm);
/* check any conditions that prevent arming */
if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) {
needs_to_arm = false;
}
if (!should_arm && !should_always_enable_pwm) {
needs_to_arm = false;
}
if (needs_to_arm && !mixer_servos_armed) {
/* need to arm, but not armed */
up_pwm_servo_arm(true);
mixer_servos_armed = true;
r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
isr_debug(5, "> PWM enabled");
} else if ((!should_arm && !should_always_enable_pwm) && mixer_servos_armed) {
} else if (!needs_to_arm && mixer_servos_armed) {
/* armed but need to disarm */
up_pwm_servo_arm(false);
mixer_servos_armed = false;
@@ -297,12 +314,17 @@ mixer_callback(uintptr_t handle,
static char mixer_text[256]; /* large enough for one mixer */
static unsigned mixer_text_length = 0;
void
int
mixer_handle_text(const void *buffer, size_t length)
{
/* do not allow a mixer change while safety off */
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
return;
return 1;
}
/* abort if we're in the mixer */
if (in_mixer) {
return 1;
}
px4io_mixdata *msg = (px4io_mixdata *)buffer;
@@ -310,7 +332,7 @@ mixer_handle_text(const void *buffer, size_t length)
isr_debug(2, "mix txt %u", length);
if (length < sizeof(px4io_mixdata))
return;
return 0;
unsigned text_length = length - sizeof(px4io_mixdata);
@@ -328,13 +350,16 @@ mixer_handle_text(const void *buffer, size_t length)
case F2I_MIXER_ACTION_APPEND:
isr_debug(2, "append %d", length);
/* disable mixing during the update */
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
/* check for overflow - this would be really fatal */
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
return;
return 0;
}
/* append mixer text and nul-terminate */
/* append mixer text and nul-terminate, guard against overflow */
memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
mixer_text_length += text_length;
mixer_text[mixer_text_length] = '\0';
@@ -369,6 +394,8 @@ mixer_handle_text(const void *buffer, size_t length)
break;
}
return 0;
}
static void
src/modules/px4iofirmware/protocol.h
+16 -4
View File
@@ -111,7 +111,6 @@
#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */
#define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */
#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */
#define PX4IO_P_STATUS_FLAGS_RC_DSM11 (1 << 13) /* DSM input is 11 bit data */
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */
@@ -128,8 +127,6 @@
#define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */
#define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */
#define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */
#define PX4IO_P_STATUS_NRSSI 9 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */
#define PX4IO_P_STATUS_RC_DATA 10 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */
/* array of post-mix actuator outputs, -10000..10000 */
#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */
@@ -140,7 +137,17 @@
/* array of raw RC input values, microseconds */
#define PX4IO_PAGE_RAW_RC_INPUT 4
#define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */
#define PX4IO_P_RAW_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT channels from here */
#define PX4IO_P_RAW_RC_FLAGS 1 /* RC detail status flags */
#define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP (1 << 0) /* single frame drop */
#define PX4IO_P_RAW_RC_FLAGS_FAILSAFE (1 << 1) /* receiver is in failsafe mode */
#define PX4IO_P_RAW_RC_FLAGS_RC_DSM11 (1 << 2) /* DSM decoding is 11 bit mode */
#define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK (1 << 3) /* Channel mapping is ok */
#define PX4IO_P_RAW_RC_NRSSI 2 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */
#define PX4IO_P_RAW_RC_DATA 3 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */
#define PX4IO_P_RAW_FRAME_COUNT 4 /* Number of total received frames (wrapping counter) */
#define PX4IO_P_RAW_LOST_FRAME_COUNT 5 /* Number of total dropped frames (wrapping counter) */
#define PX4IO_P_RAW_RC_BASE 6 /* CONFIG_RC_INPUT_COUNT channels from here */
/* array of scaled RC input values, -10000..10000 */
#define PX4IO_PAGE_RC_INPUT 5
@@ -157,6 +164,10 @@
/* setup page */
#define PX4IO_PAGE_SETUP 50
#define PX4IO_P_SETUP_FEATURES 0
#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT (1 << 0) /* enable S.Bus v1 output */
#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT (1 << 1) /* enable S.Bus v2 output */
#define PX4IO_P_SETUP_FEATURES_PWM_RSSI (1 << 2) /* enable PWM RSSI parsing */
#define PX4IO_P_SETUP_FEATURES_ADC_RSSI (1 << 3) /* enable ADC RSSI parsing */
#define PX4IO_P_SETUP_ARMING 1 /* arming controls */
#define PX4IO_P_SETUP_ARMING_IO_ARM_OK (1 << 0) /* OK to arm the IO side */
@@ -166,6 +177,7 @@
#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */
#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */
#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 6) /* Disable the IO-internal evaluation of the RC */
#define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */
#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */
src/modules/px4iofirmware/px4io.c
+43 -18
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 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
@@ -196,6 +196,11 @@ user_start(int argc, char *argv[])
POWER_SERVO(true);
#endif
/* turn off S.Bus out (if supported) */
#ifdef ENABLE_SBUS_OUT
ENABLE_SBUS_OUT(false);
#endif
/* start the safety switch handler */
safety_init();
@@ -205,6 +210,9 @@ user_start(int argc, char *argv[])
/* initialise the control inputs */
controls_init();
/* set up the ADC */
adc_init();
/* start the FMU interface */
interface_init();
@@ -223,24 +231,41 @@ user_start(int argc, char *argv[])
/* initialize PWM limit lib */
pwm_limit_init(&pwm_limit);
#if 0
/* not enough memory, lock down */
if (minfo.mxordblk < 500) {
/*
* P O L I C E L I G H T S
*
* Not enough memory, lock down.
*
* We might need to allocate mixers later, and this will
* ensure that a developer doing a change will notice
* that he just burned the remaining RAM with static
* allocations. We don't want him to be able to
* get past that point. This needs to be clearly
* documented in the dev guide.
*
*/
if (minfo.mxordblk < 600) {
lowsyslog("ERR: not enough MEM");
bool phase = false;
if (phase) {
LED_AMBER(true);
LED_BLUE(false);
} else {
LED_AMBER(false);
LED_BLUE(true);
}
while (true) {
phase = !phase;
usleep(300000);
if (phase) {
LED_AMBER(true);
LED_BLUE(false);
} else {
LED_AMBER(false);
LED_BLUE(true);
}
up_udelay(250000);
phase = !phase;
}
}
#endif
/* Start the failsafe led init */
failsafe_led_init();
/*
* Run everything in a tight loop.
@@ -270,11 +295,12 @@ user_start(int argc, char *argv[])
check_reboot();
#if 0
/* check for debug activity */
/* check for debug activity (default: none) */
show_debug_messages();
/* post debug state at ~1Hz */
/* post debug state at ~1Hz - this is via an auxiliary serial port
* DEFAULTS TO OFF!
*/
if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {
struct mallinfo minfo = mallinfo();
@@ -287,7 +313,6 @@ user_start(int argc, char *argv[])
(unsigned)minfo.mxordblk);
last_debug_time = hrt_absolute_time();
}
#endif
}
}
src/modules/px4iofirmware/px4io.h
+9 -5
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 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
@@ -96,8 +96,9 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
#define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT]
#define r_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE])
#define r_raw_rc_flags r_page_raw_rc_input[PX4IO_P_RAW_RC_FLAGS]
#define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID]
#define r_rc_values (&r_page_rc_input[PX4IO_P_RAW_RC_BASE])
#define r_rc_values (&r_page_rc_input[PX4IO_P_RC_BASE])
#define r_setup_features r_page_setup[PX4IO_P_SETUP_FEATURES]
#define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING]
@@ -115,7 +116,8 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
*/
struct sys_state_s {
volatile uint64_t rc_channels_timestamp;
volatile uint64_t rc_channels_timestamp_received;
volatile uint64_t rc_channels_timestamp_valid;
/**
* Last FMU receive time, in microseconds since system boot
@@ -160,6 +162,7 @@ extern pwm_limit_t pwm_limit;
# define PX4IO_RELAY_CHANNELS 0
# define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s))
# define ENABLE_SBUS_OUT(_s) stm32_gpiowrite(GPIO_SBUS_OENABLE, !(_s))
# define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT))
@@ -177,12 +180,13 @@ extern pwm_limit_t pwm_limit;
* Mixer
*/
extern void mixer_tick(void);
extern void mixer_handle_text(const void *buffer, size_t length);
extern int mixer_handle_text(const void *buffer, size_t length);
/**
* Safety switch/LED.
*/
extern void safety_init(void);
extern void failsafe_led_init(void);
/**
* FMU communications
@@ -213,7 +217,7 @@ extern int dsm_init(const char *device);
extern bool dsm_input(uint16_t *values, uint16_t *num_values);
extern void dsm_bind(uint16_t cmd, int pulses);
extern int sbus_init(const char *device);
extern bool sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels);
extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
/** global debug level for isr_debug() */
extern volatile uint8_t debug_level;
src/modules/px4iofirmware/registers.c
+55 -16
View File
@@ -90,8 +90,6 @@ uint16_t r_page_status[] = {
[PX4IO_P_STATUS_VSERVO] = 0,
[PX4IO_P_STATUS_VRSSI] = 0,
[PX4IO_P_STATUS_PRSSI] = 0,
[PX4IO_P_STATUS_NRSSI] = 0,
[PX4IO_P_STATUS_RC_DATA] = 0
};
/**
@@ -116,6 +114,12 @@ uint16_t r_page_servos[PX4IO_SERVO_COUNT];
uint16_t r_page_raw_rc_input[] =
{
[PX4IO_P_RAW_RC_COUNT] = 0,
[PX4IO_P_RAW_RC_FLAGS] = 0,
[PX4IO_P_RAW_RC_NRSSI] = 0,
[PX4IO_P_RAW_RC_DATA] = 0,
[PX4IO_P_RAW_FRAME_COUNT] = 0,
[PX4IO_P_RAW_LOST_FRAME_COUNT] = 0,
[PX4IO_P_RAW_RC_DATA] = 0,
[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0
};
@@ -144,7 +148,12 @@ uint16_t r_page_scratch[32];
*/
volatile uint16_t r_page_setup[] =
{
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
/* default to RSSI ADC functionality */
[PX4IO_P_SETUP_FEATURES] = PX4IO_P_SETUP_FEATURES_ADC_RSSI,
#else
[PX4IO_P_SETUP_FEATURES] = 0,
#endif
[PX4IO_P_SETUP_ARMING] = 0,
[PX4IO_P_SETUP_PWM_RATES] = 0,
[PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50,
@@ -162,14 +171,22 @@ volatile uint16_t r_page_setup[] =
[PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
};
#define PX4IO_P_SETUP_FEATURES_VALID (0)
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
#define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \
PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \
PX4IO_P_SETUP_FEATURES_ADC_RSSI | \
PX4IO_P_SETUP_FEATURES_PWM_RSSI)
#else
#define PX4IO_P_SETUP_FEATURES_VALID 0
#endif
#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
PX4IO_P_SETUP_ARMING_IO_ARM_OK | \
PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)
PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
PX4IO_P_SETUP_ARMING_LOCKDOWN)
#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
@@ -382,7 +399,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
/* handle text going to the mixer parser */
case PX4IO_PAGE_MIXERLOAD:
mixer_handle_text(values, num_values * sizeof(*values));
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
(r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
return mixer_handle_text(values, num_values * sizeof(*values));
}
break;
default:
@@ -435,9 +455,35 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
case PX4IO_P_SETUP_FEATURES:
value &= PX4IO_P_SETUP_FEATURES_VALID;
r_setup_features = value;
/* no implemented feature selection at this point */
/* some of the options conflict - give S.BUS out precedence, then ADC RSSI, then PWM RSSI */
/* switch S.Bus output pin as needed */
#ifdef ENABLE_SBUS_OUT
ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT));
/* disable the conflicting options */
if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) {
value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | PX4IO_P_SETUP_FEATURES_ADC_RSSI);
}
#endif
/* disable the conflicting options with ADC RSSI */
if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
PX4IO_P_SETUP_FEATURES_SBUS1_OUT |
PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
}
/* disable the conflicting options with PWM RSSI (without effect here, but for completeness) */
if (value & (PX4IO_P_SETUP_FEATURES_PWM_RSSI)) {
value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI |
PX4IO_P_SETUP_FEATURES_SBUS1_OUT |
PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
}
/* apply changes */
r_setup_features = value;
break;
@@ -453,11 +499,6 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
* lockup of the IO arming state.
*/
// XXX do not reset IO's safety state by FMU for now
// if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
// r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
// }
if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
}
@@ -509,8 +550,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
case PX4IO_P_SETUP_REBOOT_BL:
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ||
(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
(r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
// don't allow reboot while armed
break;
}
@@ -540,8 +580,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
* do not allow a RC config change while outputs armed
*/
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ||
(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
(r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
break;
}
src/modules/px4iofirmware/safety.c
+6 -3
View File
@@ -45,7 +45,6 @@
#include "px4io.h"
static struct hrt_call arming_call;
static struct hrt_call heartbeat_call;
static struct hrt_call failsafe_call;
/*
@@ -84,7 +83,11 @@ safety_init(void)
{
/* arrange for the button handler to be called at 10Hz */
hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL);
}
void
failsafe_led_init(void)
{
/* arrange for the failsafe blinker to be called at 8Hz */
hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL);
}
@@ -165,8 +168,8 @@ failsafe_blink(void *arg)
/* indicate that a serious initialisation error occured */
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) {
LED_AMBER(true);
return;
}
return;
}
static bool failsafe = false;
src/modules/px4iofirmware/sbus.c
+36 -12
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 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
@@ -87,7 +87,7 @@ static unsigned partial_frame_count;
unsigned sbus_frame_drops;
static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels);
static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
int
sbus_init(const char *device)
@@ -118,7 +118,7 @@ sbus_init(const char *device)
}
bool
sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels)
sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels)
{
ssize_t ret;
hrt_abstime now;
@@ -175,7 +175,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_
* decode it.
*/
partial_frame_count = 0;
return sbus_decode(now, values, num_values, rssi, max_channels);
return sbus_decode(now, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels);
}
/*
@@ -215,14 +215,36 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
};
static bool
sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values)
sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values)
{
/* check frame boundary markers to avoid out-of-sync cases */
if ((frame[0] != 0x0f) || (frame[24] != 0x00)) {
if ((frame[0] != 0x0f)) {
sbus_frame_drops++;
return false;
}
switch (frame[24]) {
case 0x00:
/* this is S.BUS 1 */
break;
case 0x03:
/* S.BUS 2 SLOT0: RX battery and external voltage */
break;
case 0x83:
/* S.BUS 2 SLOT1 */
break;
case 0x43:
case 0xC3:
case 0x23:
case 0xA3:
case 0x63:
case 0xE3:
break;
default:
/* we expect one of the bits above, but there are some we don't know yet */
break;
}
/* we have received something we think is a frame */
last_frame_time = frame_time;
@@ -267,20 +289,22 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint
/* decode and handle failsafe and frame-lost flags */
if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */
/* report that we failed to read anything valid off the receiver */
*rssi = 0;
return false;
*sbus_failsafe = true;
*sbus_frame_drop = true;
}
else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */
/* set a special warning flag or try to calculate some kind of RSSI information - to be implemented
/* set a special warning flag
*
* Attention! This flag indicates a skipped frame only, not a total link loss! Handling this
* condition as fail-safe greatly reduces the reliability and range of the radio link,
* e.g. by prematurely issueing return-to-launch!!! */
*rssi = 100; // XXX magic number indicating bad signal, but not a signal loss (yet)
*sbus_failsafe = false;
*sbus_frame_drop = true;
} else {
*sbus_failsafe = false;
*sbus_frame_drop = false;
}
*rssi = 255;
return true;
}
src/modules/sdlog2/sdlog2.c
+202 -110
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
* Anton Babushkin <anton.babushkin@me.com>
*
@@ -85,13 +85,13 @@
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <version/version.h>
#include <mavlink/mavlink_log.h>
#include "logbuffer.h"
#include "sdlog2_format.h"
#include "sdlog2_messages.h"
#include "sdlog2_version.h"
#define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \
log_msgs_written++; \
@@ -108,13 +108,13 @@ static bool main_thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
static bool logwriter_should_exit = false; /**< Logwriter thread exit flag */
static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log folders */
static const int MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */
static const int MAX_NO_LOGFILE = 999; /**< Maximum number of log files */
static const int LOG_BUFFER_SIZE_DEFAULT = 8192;
static const int MAX_WRITE_CHUNK = 512;
static const int MIN_BYTES_TO_WRITE = 512;
static const char *mountpoint = "/fs/microsd";
static const char *log_root = "/fs/microsd/log";
static int mavlink_fd = -1;
struct logbuffer_s lb;
@@ -122,14 +122,17 @@ struct logbuffer_s lb;
static pthread_mutex_t logbuffer_mutex;
static pthread_cond_t logbuffer_cond;
static char folder_path[64];
static char log_dir[32];
/* statistics counters */
static unsigned long log_bytes_written = 0;
static uint64_t start_time = 0;
static unsigned long log_bytes_written = 0;
static unsigned long log_msgs_written = 0;
static unsigned long log_msgs_skipped = 0;
/* GPS time, used for log files naming */
static uint64_t gps_time = 0;
/* current state of logging */
static bool logging_enabled = false;
/* enable logging on start (-e option) */
@@ -138,11 +141,14 @@ static bool log_on_start = false;
static bool log_when_armed = false;
/* delay = 1 / rate (rate defined by -r option) */
static useconds_t sleep_delay = 0;
/* use date/time for naming directories and files (-t option) */
static bool log_name_timestamp = false;
/* helper flag to track system state changes */
static bool flag_system_armed = false;
static pthread_t logwriter_pthread = 0;
static pthread_attr_t logwriter_attr;
/**
* Log buffer writing thread. Open and close file here.
@@ -203,14 +209,14 @@ static void handle_command(struct vehicle_command_s *cmd);
static void handle_status(struct vehicle_status_s *cmd);
/**
* Create folder for current logging session. Store folder name in 'log_folder'.
* Create dir for current logging session. Store dir name in 'log_dir'.
*/
static int create_logfolder(void);
static int create_log_dir(void);
/**
* Select first free log file name and open it.
*/
static int open_logfile(void);
static int open_log_file(void);
static void
sdlog2_usage(const char *reason)
@@ -218,11 +224,12 @@ sdlog2_usage(const char *reason)
if (reason)
fprintf(stderr, "%s\n", reason);
errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a\n"
errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t\n"
"\t-r\tLog rate in Hz, 0 means unlimited rate\n"
"\t-b\tLog buffer size in KiB, default is 8\n"
"\t-e\tEnable logging by default (if not, can be started by command)\n"
"\t-a\tLog only when armed (can be still overriden by command)\n");
"\t-a\tLog only when armed (can be still overriden by command)\n"
"\t-t\tUse date/time for naming log directories and files\n");
}
/**
@@ -280,82 +287,112 @@ int sdlog2_main(int argc, char *argv[])
exit(1);
}
int create_logfolder()
int create_log_dir()
{
/* make folder on sdcard */
uint16_t folder_number = 1; // start with folder sess001
/* create dir on sdcard if needed */
uint16_t dir_number = 1; // start with dir sess001
int mkdir_ret;
/* look for the next folder that does not exist */
while (folder_number <= MAX_NO_LOGFOLDER) {
/* set up folder path: e.g. /fs/microsd/sess001 */
sprintf(folder_path, "%s/sess%03u", mountpoint, folder_number);
mkdir_ret = mkdir(folder_path, S_IRWXU | S_IRWXG | S_IRWXO);
/* the result is -1 if the folder exists */
if (log_name_timestamp && gps_time != 0) {
/* use GPS date for log dir naming: e.g. /fs/microsd/2014-01-19 */
time_t gps_time_sec = gps_time / 1000000;
struct tm t;
gmtime_r(&gps_time_sec, &t);
int n = snprintf(log_dir, sizeof(log_dir), "%s/", log_root);
strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &t);
mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO);
if (mkdir_ret == 0) {
/* folder does not exist, success */
break;
if (mkdir_ret == OK) {
warnx("log dir created: %s", log_dir);
} else if (mkdir_ret == -1) {
/* folder exists already */
folder_number++;
} else if (errno != EEXIST) {
warn("failed creating new dir: %s", log_dir);
return -1;
}
} else {
/* look for the next dir that does not exist */
while (dir_number <= MAX_NO_LOGFOLDER) {
/* format log dir: e.g. /fs/microsd/sess001 */
sprintf(log_dir, "%s/sess%03u", log_root, dir_number);
mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO);
if (mkdir_ret == 0) {
warnx("log dir created: %s", log_dir);
break;
} else if (errno != EEXIST) {
warn("failed creating new dir: %s", log_dir);
return -1;
}
/* dir exists already */
dir_number++;
continue;
}
} else {
warn("failed creating new folder");
if (dir_number >= MAX_NO_LOGFOLDER) {
/* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */
warnx("all %d possible dirs exist already", MAX_NO_LOGFOLDER);
return -1;
}
}
if (folder_number >= MAX_NO_LOGFOLDER) {
/* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */
warnx("all %d possible folders exist already.", MAX_NO_LOGFOLDER);
return -1;
}
/* print logging path, important to find log file later */
warnx("log dir: %s", log_dir);
mavlink_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir);
return 0;
}
int open_logfile()
int open_log_file()
{
/* make folder on sdcard */
uint16_t file_number = 1; // start with file log001
/* string to hold the path to the log */
char path_buf[64] = "";
char log_file_name[16] = "";
char log_file_path[48] = "";
int fd = 0;
if (log_name_timestamp && gps_time != 0) {
/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
time_t gps_time_sec = gps_time / 1000000;
struct tm t;
gmtime_r(&gps_time_sec, &t);
strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.bin", &t);
snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name);
/* look for the next file that does not exist */
while (file_number <= MAX_NO_LOGFILE) {
/* set up file path: e.g. /fs/microsd/sess001/log001.bin */
sprintf(path_buf, "%s/log%03u.bin", folder_path, file_number);
} else {
uint16_t file_number = 1; // start with file log001
/* look for the next file that does not exist */
while (file_number <= MAX_NO_LOGFILE) {
/* format log file path: e.g. /fs/microsd/sess001/log001.bin */
snprintf(log_file_name, sizeof(log_file_name), "log%03u.bin", file_number);
snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name);
if (!file_exist(log_file_path)) {
break;
}
if (file_exist(path_buf)) {
file_number++;
continue;
}
fd = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC);
if (fd == 0) {
warn("opening %s failed", path_buf);
if (file_number > MAX_NO_LOGFILE) {
/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
warnx("all %d possible files exist already", MAX_NO_LOGFILE);
return -1;
}
warnx("logging to: %s.", path_buf);
mavlink_log_info(mavlink_fd, "[sdlog2] log: %s", path_buf);
return fd;
}
if (file_number > MAX_NO_LOGFILE) {
/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
warnx("all %d possible files exist already.", MAX_NO_LOGFILE);
return -1;
int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC);
if (fd < 0) {
warn("failed opening log: %s", log_file_name);
mavlink_log_info(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
} else {
warnx("log file: %s", log_file_name);
mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name);
}
return 0;
return fd;
}
static void *logwriter_thread(void *arg)
@@ -363,9 +400,12 @@ static void *logwriter_thread(void *arg)
/* set name */
prctl(PR_SET_NAME, "sdlog2_writer", 0);
struct logbuffer_s *logbuf = (struct logbuffer_s *)arg;
int log_fd = open_log_file();
int log_fd = open_logfile();
if (log_fd < 0)
return;
struct logbuffer_s *logbuf = (struct logbuffer_s *)arg;
/* write log messages formats, version and parameters */
log_bytes_written += write_formats(log_fd);
@@ -443,14 +483,20 @@ static void *logwriter_thread(void *arg)
fsync(log_fd);
close(log_fd);
return OK;
return;
}
void sdlog2_start_log()
{
warnx("start logging.");
warnx("start logging");
mavlink_log_info(mavlink_fd, "[sdlog2] start logging");
/* create log dir if needed */
if (create_log_dir() != 0) {
mavlink_log_critical(mavlink_fd, "[sdlog2] error creating log dir");
errx(1, "error creating log dir");
}
/* initialize statistics counter */
log_bytes_written = 0;
start_time = hrt_absolute_time();
@@ -458,30 +504,28 @@ void sdlog2_start_log()
log_msgs_skipped = 0;
/* initialize log buffer emptying thread */
pthread_attr_t receiveloop_attr;
pthread_attr_init(&receiveloop_attr);
pthread_attr_init(&logwriter_attr);
struct sched_param param;
/* low priority, as this is expensive disk I/O */
param.sched_priority = SCHED_PRIORITY_DEFAULT - 40;
(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
(void)pthread_attr_setschedparam(&logwriter_attr, &param);
pthread_attr_setstacksize(&receiveloop_attr, 2048);
pthread_attr_setstacksize(&logwriter_attr, 2048);
logwriter_should_exit = false;
/* start log buffer emptying thread */
if (0 != pthread_create(&logwriter_pthread, &receiveloop_attr, logwriter_thread, &lb)) {
if (0 != pthread_create(&logwriter_pthread, &logwriter_attr, logwriter_thread, &lb)) {
errx(1, "error creating logwriter thread");
}
logging_enabled = true;
// XXX we have to destroy the attr at some point
}
void sdlog2_stop_log()
{
warnx("stop logging.");
warnx("stop logging");
mavlink_log_info(mavlink_fd, "[sdlog2] stop logging");
logging_enabled = false;
@@ -501,6 +545,7 @@ void sdlog2_stop_log()
}
logwriter_pthread = 0;
pthread_attr_destroy(&logwriter_attr);
sdlog2_status();
}
@@ -569,8 +614,8 @@ int write_parameters(int fd)
}
case PARAM_TYPE_FLOAT:
param_get(param, &value);
break;
param_get(param, &value);
break;
default:
break;
@@ -588,18 +633,25 @@ int sdlog2_thread_main(int argc, char *argv[])
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
if (mavlink_fd < 0) {
warnx("failed to open MAVLink log stream, start mavlink app first.");
warnx("failed to open MAVLink log stream, start mavlink app first");
}
/* log buffer size */
int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT;
logging_enabled = false;
log_on_start = false;
log_when_armed = false;
log_name_timestamp = false;
flag_system_armed = false;
/* work around some stupidity in task_create's argv handling */
argc -= 2;
argv += 2;
int ch;
while ((ch = getopt(argc, argv, "r:b:ea")) != EOF) {
while ((ch = getopt(argc, argv, "r:b:eat")) != EOF) {
switch (ch) {
case 'r': {
unsigned long r = strtoul(optarg, NULL, 10);
@@ -632,49 +684,52 @@ int sdlog2_thread_main(int argc, char *argv[])
log_when_armed = true;
break;
case 't':
log_name_timestamp = true;
break;
case '?':
if (optopt == 'c') {
warnx("Option -%c requires an argument.", optopt);
warnx("option -%c requires an argument", optopt);
} else if (isprint(optopt)) {
warnx("Unknown option `-%c'.", optopt);
warnx("unknown option `-%c'", optopt);
} else {
warnx("Unknown option character `\\x%x'.", optopt);
warnx("unknown option character `\\x%x'", optopt);
}
default:
sdlog2_usage("unrecognized flag");
errx(1, "exiting.");
errx(1, "exiting");
}
}
if (!file_exist(mountpoint)) {
errx(1, "logging mount point %s not present, exiting.", mountpoint);
}
if (create_logfolder()) {
errx(1, "unable to create logging folder, exiting.");
gps_time = 0;
/* create log root dir */
int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO);
if (mkdir_ret != 0 && errno != EEXIST) {
err("failed creating log root dir: %s", log_root);
}
/* copy conversion scripts */
const char *converter_in = "/etc/logging/conv.zip";
char *converter_out = malloc(120);
sprintf(converter_out, "%s/conv.zip", folder_path);
char *converter_out = malloc(64);
snprintf(converter_out, 64, "%s/conv.zip", log_root);
if (file_copy(converter_in, converter_out)) {
errx(1, "unable to copy conversion scripts, exiting.");
if (file_copy(converter_in, converter_out) != OK) {
warn("unable to copy conversion scripts");
}
free(converter_out);
/* only print logging path, important to find log file later */
warnx("logging to directory: %s", folder_path);
/* initialize log buffer with specified size */
warnx("log buffer size: %i bytes.", log_buffer_size);
warnx("log buffer size: %i bytes", log_buffer_size);
if (OK != logbuffer_init(&lb, log_buffer_size)) {
errx(1, "can't allocate log buffer, exiting.");
errx(1, "can't allocate log buffer, exiting");
}
struct vehicle_status_s buf_status;
@@ -702,6 +757,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct airspeed_s airspeed;
struct esc_status_s esc;
struct vehicle_global_velocity_setpoint_s global_vel_sp;
struct battery_status_s battery;
} buf;
memset(&buf, 0, sizeof(buf));
@@ -726,6 +782,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int airspeed_sub;
int esc_sub;
int global_vel_sp_sub;
int battery_sub;
} subs;
/* log message buffer: header + body */
@@ -752,6 +809,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct log_GPSP_s log_GPSP;
struct log_ESC_s log_ESC;
struct log_GVSP_s log_GVSP;
struct log_BATT_s log_BATT;
} body;
} log_msg = {
LOG_PACKET_HEADER_INIT(0)
@@ -760,9 +818,9 @@ int sdlog2_thread_main(int argc, char *argv[])
memset(&log_msg.body, 0, sizeof(log_msg.body));
/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
/* number of subscriptions */
const ssize_t fdsc = 19;
/* sanity check variable and index */
/* number of messages */
const ssize_t fdsc = 25;
/* Sanity check variable and index */
ssize_t fdsc_count = 0;
/* file descriptors to wait for */
struct pollfd fds[fdsc];
@@ -881,12 +939,18 @@ int sdlog2_thread_main(int argc, char *argv[])
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* --- BATTERY --- */
subs.battery_sub = orb_subscribe(ORB_ID(battery_status));
fds[fdsc_count].fd = subs.battery_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
/* WARNING: If you get the error message below,
* then the number of registered messages (fdsc)
* differs from the number of messages in the above list.
*/
if (fdsc_count > fdsc) {
warn("WARNING: Not enough space for poll fds allocated. Check %s:%d.", __FILE__, __LINE__);
warn("WARNING: Not enough space for poll fds allocated. Check %s:%d", __FILE__, __LINE__);
fdsc_count = fdsc;
}
@@ -910,19 +974,27 @@ int sdlog2_thread_main(int argc, char *argv[])
hrt_abstime differential_pressure_timestamp = 0;
/* enable logging on start if needed */
if (log_on_start)
if (log_on_start) {
/* check GPS topic to get GPS time */
if (log_name_timestamp) {
if (OK == orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos)) {
gps_time = buf.gps_pos.time_gps_usec;
}
}
sdlog2_start_log();
}
while (!main_thread_should_exit) {
/* decide use usleep() or blocking poll() */
bool use_sleep = sleep_delay > 0 && logging_enabled;
/* poll all topics if logging enabled or only management (first 2) if not */
int poll_ret = poll(fds, logging_enabled ? fdsc_count : 2, use_sleep ? 0 : poll_timeout);
int poll_ret = poll(fds, logging_enabled ? fdsc_count : 3, use_sleep ? 0 : poll_timeout);
/* handle the poll result */
if (poll_ret < 0) {
warnx("ERROR: poll error, stop logging.");
warnx("ERROR: poll error, stop logging");
main_thread_should_exit = true;
} else if (poll_ret > 0) {
@@ -951,6 +1023,17 @@ int sdlog2_thread_main(int argc, char *argv[])
handled_topics++;
}
/* --- GPS POSITION - LOG MANAGEMENT --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
if (log_name_timestamp) {
gps_time = buf.gps_pos.time_gps_usec;
}
handled_topics++;
}
if (!logging_enabled || !check_data || handled_topics >= poll_ret) {
continue;
}
@@ -971,8 +1054,6 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state;
log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state;
log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage;
log_msg.body.log_STAT.battery_current = buf_status.battery_current;
log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning;
log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed;
@@ -981,7 +1062,7 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- GPS POSITION --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
// Don't orb_copy, it's already done few lines above
log_msg.msg_type = LOG_GPS_MSG;
log_msg.body.log_GPS.gps_time = buf.gps_pos.time_gps_usec;
log_msg.body.log_GPS.fix_type = buf.gps_pos.fix_type;
@@ -1238,6 +1319,17 @@ int sdlog2_thread_main(int argc, char *argv[])
LOGBUFFER_WRITE_AND_COUNT(GVSP);
}
/* --- BATTERY --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(battery_status), subs.battery_sub, &buf.battery);
log_msg.msg_type = LOG_BATT_MSG;
log_msg.body.log_BATT.voltage = buf.battery.voltage_v;
log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v;
log_msg.body.log_BATT.current = buf.battery.current_a;
log_msg.body.log_BATT.discharged = buf.battery.discharged_mah;
LOGBUFFER_WRITE_AND_COUNT(BATT);
}
/* signal the other thread new data, but not yet unlock */
if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
/* only request write if several packets can be written at once */
@@ -1261,7 +1353,7 @@ int sdlog2_thread_main(int argc, char *argv[])
free(lb.data);
warnx("exiting.");
warnx("exiting");
thread_running = false;
@@ -1274,8 +1366,8 @@ void sdlog2_status()
float mebibytes = kibibytes / 1024.0f;
float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f;
warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs.", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped);
mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs.", log_msgs_written, log_msgs_skipped);
warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped);
mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped);
}
/**
@@ -1294,7 +1386,7 @@ int file_copy(const char *file_old, const char *file_new)
int ret = 0;
if (source == NULL) {
warnx("failed opening input file to copy.");
warnx("failed opening input file to copy");
return 1;
}
@@ -1302,7 +1394,7 @@ int file_copy(const char *file_old, const char *file_new)
if (target == NULL) {
fclose(source);
warnx("failed to open output file to copy.");
warnx("failed to open output file to copy");
return 1;
}
@@ -1313,7 +1405,7 @@ int file_copy(const char *file_old, const char *file_new)
ret = fwrite(buf, 1, nread, target);
if (ret <= 0) {
warnx("error writing file.");
warnx("error writing file");
ret = 1;
break;
}
src/modules/sdlog2/sdlog2_messages.h
+11 -3
View File
@@ -148,8 +148,6 @@ struct log_STAT_s {
uint8_t main_state;
uint8_t navigation_state;
uint8_t arming_state;
float battery_voltage;
float battery_current;
float battery_remaining;
uint8_t battery_warning;
uint8_t landed;
@@ -248,6 +246,15 @@ struct log_GVSP_s {
float vz;
};
/* --- BATT - BATTERY --- */
#define LOG_BATT_MSG 20
struct log_BATT_s {
float voltage;
float voltage_filtered;
float current;
float discharged;
};
/* --- TIME - TIME STAMP --- */
#define LOG_TIME_MSG 129
struct log_TIME_s {
@@ -281,7 +288,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
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, "BBBfffBB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn,Landed"),
LOG_FORMAT(STAT, "BBBfBB", "MainState,NavState,ArmState,BatRem,BatWarn,Landed"),
LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
@@ -291,6 +298,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
/* system-level messages, ID >= 0x80 */
// FMT: don't write format of format message, it's useless
LOG_FORMAT(TIME, "Q", "StartTime"),
src/modules/sdlog2/sdlog2_version.h
-62
View File
@@ -1,62 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Anton Babushkin <anton.babushkin@me.com>
*
* 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 sdlog2_version.h
*
* Tools for system version detection.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef SDLOG2_VERSION_H_
#define SDLOG2_VERSION_H_
/*
GIT_VERSION is defined at build time via a Makefile call to the
git command line.
*/
#define FREEZE_STR(s) #s
#define STRINGIFY(s) FREEZE_STR(s)
#define FW_GIT STRINGIFY(GIT_VERSION)
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
#define HW_ARCH "PX4FMU_V1"
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
#define HW_ARCH "PX4FMU_V2"
#endif
#endif /* SDLOG2_VERSION_H_ */
src/modules/sensors/sensor_params.c
+62 -2
View File
@@ -379,15 +379,75 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
/* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */
PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f);
#endif
PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124); /* scaling for 3DR power brick */
/**
* Roll control channel mapping.
*
* The channel index (starting from 1 for channel 1) indicates
* which channel should be used for reading roll inputs from.
* A value of zero indicates the switch is not assigned.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
/**
* Pitch control channel mapping.
*
* The channel index (starting from 1 for channel 1) indicates
* which channel should be used for reading pitch inputs from.
* A value of zero indicates the switch is not assigned.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
/**
* Throttle control channel mapping.
*
* The channel index (starting from 1 for channel 1) indicates
* which channel should be used for reading throttle inputs from.
* A value of zero indicates the switch is not assigned.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
/**
* Yaw control channel mapping.
*
* The channel index (starting from 1 for channel 1) indicates
* which channel should be used for reading yaw inputs from.
* A value of zero indicates the switch is not assigned.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5);
/**
* Mode switch channel mapping.
*
* This is the main flight mode selector.
* The channel index (starting from 1 for channel 1) indicates
* which channel should be used for deciding about the main mode.
* A value of zero indicates the switch is not assigned.
*
* @min 0
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6);
PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0);
//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
src/modules/sensors/sensors.cpp
+68 -35
View File
@@ -115,6 +115,7 @@
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
#define ADC_BATTERY_VOLTAGE_CHANNEL 10
#define ADC_BATTERY_CURRENT_CHANNEL -1
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11
#endif
@@ -125,10 +126,8 @@
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15
#endif
#define BAT_VOL_INITIAL 0.f
#define BAT_VOL_LOWPASS_1 0.99f
#define BAT_VOL_LOWPASS_2 0.01f
#define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f
#define BATT_V_LOWPASS 0.001f
#define BATT_V_IGNORE_THRESHOLD 3.5f
/**
* HACK - true temperature is much less than indicated temperature in baro,
@@ -216,6 +215,9 @@ private:
math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
bool _mag_is_external; /**< true if the active mag is on an external board */
uint64_t _battery_discharged; /**< battery discharged current in mA*ms */
hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */
struct {
float min[_rc_max_chan_count];
float trim[_rc_max_chan_count];
@@ -266,6 +268,7 @@ private:
float rc_fs_thr;
float battery_voltage_scaling;
float battery_current_scaling;
} _parameters; /**< local copies of interesting parameters */
@@ -315,6 +318,7 @@ private:
param_t rc_fs_thr;
param_t battery_voltage_scaling;
param_t battery_current_scaling;
param_t board_rotation;
param_t external_mag_rotation;
@@ -468,7 +472,9 @@ Sensors::Sensors() :
_board_rotation(3, 3),
_external_mag_rotation(3, 3),
_mag_is_external(false)
_mag_is_external(false),
_battery_discharged(0),
_battery_current_timestamp(0)
{
/* basic r/c parameters */
@@ -561,6 +567,7 @@ Sensors::Sensors() :
_parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA");
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
_parameter_handles.battery_current_scaling = param_find("BAT_C_SCALING");
/* rotations */
_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
@@ -741,6 +748,11 @@ Sensors::parameters_update()
warnx("Failed updating voltage scaling param");
}
/* scaling of ADC ticks to battery current */
if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) {
warnx("Failed updating current scaling param");
}
param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation));
@@ -786,7 +798,6 @@ Sensors::accel_init()
#endif
warnx("using system accel");
close(fd);
}
}
@@ -826,7 +837,6 @@ Sensors::gyro_init()
#endif
warnx("using system gyro");
close(fd);
}
}
@@ -1158,17 +1168,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
if (!_publishing)
return;
hrt_abstime t = hrt_absolute_time();
/* rate limit to 100 Hz */
if (hrt_absolute_time() - _last_adc >= 10000) {
if (t - _last_adc >= 10000) {
/* make space for a maximum of eight channels */
struct adc_msg_s buf_adc[8];
/* read all channels available */
int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc));
for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
if (ret >= (int)sizeof(buf_adc[0])) {
if (ret >= (int)sizeof(buf_adc[0])) {
for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
/* Save raw voltage values */
if (i < (sizeof(raw.adc_voltage_v)) / sizeof(raw.adc_voltage_v[0])) {
raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f);
@@ -1179,27 +1188,40 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
/* Voltage in volts */
float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling);
if (voltage > VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
if (voltage > BATT_V_IGNORE_THRESHOLD) {
_battery_status.voltage_v = voltage;
/* one-time initialization of low-pass value to avoid long init delays */
if (_battery_status.voltage_v < 3.0f) {
_battery_status.voltage_v = voltage;
if (_battery_status.voltage_filtered_v < BATT_V_IGNORE_THRESHOLD) {
_battery_status.voltage_filtered_v = voltage;
}
_battery_status.timestamp = hrt_absolute_time();
_battery_status.voltage_v = (BAT_VOL_LOWPASS_1 * (_battery_status.voltage_v + BAT_VOL_LOWPASS_2 * voltage));;
/* current and discharge are unknown */
_battery_status.current_a = -1.0f;
_battery_status.discharged_mah = -1.0f;
_battery_status.timestamp = t;
_battery_status.voltage_filtered_v += (voltage - _battery_status.voltage_filtered_v) * BATT_V_LOWPASS;
/* announce the battery voltage if needed, just publish else */
if (_battery_pub > 0) {
orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
} else {
/* mark status as invalid */
_battery_status.voltage_v = -1.0f;
_battery_status.voltage_filtered_v = -1.0f;
}
} else {
_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
} else if (ADC_BATTERY_CURRENT_CHANNEL == buf_adc[i].am_channel) {
/* handle current only if voltage is valid */
if (_battery_status.voltage_v > 0.0f) {
float current = (buf_adc[i].am_data * _parameters.battery_current_scaling);
/* check measured current value */
if (current >= 0.0f) {
_battery_status.timestamp = t;
_battery_status.current_a = current;
if (_battery_current_timestamp != 0) {
/* initialize discharged value */
if (_battery_status.discharged_mah < 0.0f)
_battery_status.discharged_mah = 0.0f;
_battery_discharged += current * (t - _battery_current_timestamp);
_battery_status.discharged_mah = ((float) _battery_discharged) / 3600000.0f;
}
}
}
_battery_current_timestamp = t;
} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
@@ -1215,7 +1237,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor
_diff_pres.timestamp = hrt_absolute_time();
_diff_pres.timestamp = t;
_diff_pres.differential_pressure_pa = diff_pres_pa;
_diff_pres.voltage = voltage;
@@ -1228,8 +1250,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
}
}
}
}
_last_adc = t;
if (_battery_status.voltage_v > 0.0f) {
/* announce the battery status if needed, just publish else */
if (_battery_pub > 0) {
orb_publish(ORB_ID(battery_status), _battery_pub, &_battery_status);
_last_adc = hrt_absolute_time();
} else {
_battery_pub = orb_advertise(ORB_ID(battery_status), &_battery_status);
}
}
}
}
@@ -1247,6 +1277,9 @@ Sensors::rc_poll()
orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
if (rc_input.rc_lost)
return;
struct manual_control_setpoint_s manual_control;
struct actuator_controls_s actuator_group_3;
@@ -1291,7 +1324,7 @@ Sensors::rc_poll()
channel_limit = _rc_max_chan_count;
/* we are accepting this message */
_rc_last_valid = rc_input.timestamp;
_rc_last_valid = rc_input.timestamp_last_signal;
/* Read out values from raw message */
for (unsigned int i = 0; i < channel_limit; i++) {
@@ -1340,9 +1373,9 @@ Sensors::rc_poll()
}
_rc.chan_count = rc_input.channel_count;
_rc.timestamp = rc_input.timestamp;
_rc.timestamp = rc_input.timestamp_last_signal;
manual_control.timestamp = rc_input.timestamp;
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);
@@ -1476,9 +1509,6 @@ void
Sensors::task_main()
{
/* inform about start */
warnx("Initializing..");
/* start individual sensors */
accel_init();
gyro_init();
@@ -1517,7 +1547,10 @@ Sensors::task_main()
raw.adc_voltage_v[3] = 0.0f;
memset(&_battery_status, 0, sizeof(_battery_status));
_battery_status.voltage_v = BAT_VOL_INITIAL;
_battery_status.voltage_v = -1.0f;
_battery_status.voltage_filtered_v = -1.0f;
_battery_status.current_a = -1.0f;
_battery_status.discharged_mah = -1.0f;
/* get a set of initial values */
accel_poll(raw);
src/modules/systemlib/cpuload.c
+1 -2
View File
@@ -93,8 +93,7 @@ void cpuload_initialize_once()
#endif /* CONFIG_SCHED_WORKQUEUE */
// perform static initialization of "system" threads
for (system_load.total_count = 0; system_load.total_count < static_tasks_count; system_load.total_count++)
{
for (system_load.total_count = 0; system_load.total_count < static_tasks_count; system_load.total_count++) {
system_load.tasks[system_load.total_count].start_time = now;
system_load.tasks[system_load.total_count].total_runtime = 0;
system_load.tasks[system_load.total_count].curr_start_time = 0;
src/modules/systemlib/cpuload.h
+6 -6
View File
@@ -40,15 +40,15 @@ __BEGIN_DECLS
#include <nuttx/sched.h>
struct system_load_taskinfo_s {
uint64_t total_runtime; ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load
uint64_t curr_start_time; ///< Start time of the current scheduling slot
uint64_t start_time; ///< FIRST start time of task
FAR struct tcb_s *tcb; ///<
bool valid; ///< Task is currently active / valid
uint64_t total_runtime; ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load
uint64_t curr_start_time; ///< Start time of the current scheduling slot
uint64_t start_time; ///< FIRST start time of task
FAR struct tcb_s *tcb; ///<
bool valid; ///< Task is currently active / valid
};
struct system_load_s {
uint64_t start_time; ///< Global start time of measurements
uint64_t start_time; ///< Global start time of measurements
struct system_load_taskinfo_s tasks[CONFIG_MAX_TASKS];
uint8_t initialized;
int total_count;
src/modules/systemlib/module.mk
+2 -1
View File
@@ -51,5 +51,6 @@ SRCS = err.c \
mavlink_log.c \
rc_check.c \
otp.c \
board_serial.c
board_serial.c \
pwm_limit/pwm_limit.c
src/modules/systemlib/pwm_limit/pwm_limit.c
+64 -46
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
* Author: Julian Oes <joes@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
@@ -44,38 +44,53 @@
#include <math.h>
#include <stdbool.h>
#include <drivers/drv_hrt.h>
#include <stdio.h>
void pwm_limit_init(pwm_limit_t *limit)
{
limit->state = LIMIT_STATE_OFF;
limit->state = PWM_LIMIT_STATE_INIT;
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)
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, const 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();
case PWM_LIMIT_STATE_INIT:
if (armed) {
/* set arming time for the first call */
if (limit->time_armed == 0) {
limit->time_armed = hrt_absolute_time();
}
if (hrt_elapsed_time(&limit->time_armed) >= INIT_TIME_US) {
limit->state = PWM_LIMIT_STATE_OFF;
}
}
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;
case PWM_LIMIT_STATE_OFF:
if (armed) {
limit->state = PWM_LIMIT_STATE_RAMP;
/* reset arming time, used for ramp timing */
limit->time_armed = hrt_absolute_time();
}
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;
case PWM_LIMIT_STATE_RAMP:
if (!armed) {
limit->state = PWM_LIMIT_STATE_OFF;
} else if (hrt_elapsed_time(&limit->time_armed) >= RAMP_TIME_US) {
limit->state = PWM_LIMIT_STATE_ON;
}
break;
case LIMIT_STATE_ON:
if (!armed)
limit->state = LIMIT_STATE_OFF;
case PWM_LIMIT_STATE_ON:
if (!armed) {
limit->state = PWM_LIMIT_STATE_OFF;
}
break;
default:
break;
@@ -86,44 +101,47 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
/* then set effective_pwm based on state */
switch (limit->state) {
case LIMIT_STATE_OFF:
case LIMIT_STATE_INIT:
case PWM_LIMIT_STATE_OFF:
case PWM_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:
case PWM_LIMIT_STATE_RAMP:
{
hrt_abstime diff = hrt_elapsed_time(&limit->time_armed);
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) {
progress = diff * 10000 / RAMP_TIME_US;
/* safeguard against overflows */
uint16_t disarmed = disarmed_pwm[i];
if (disarmed > min_pwm[i])
disarmed = min_pwm[i];
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) {
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];
/* safeguard against overflows */
unsigned disarmed = disarmed_pwm[i];
if (disarmed > min_pwm[i]) {
disarmed = min_pwm[i];
}
unsigned 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;
}
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:
case PWM_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:
src/modules/systemlib/pwm_limit/pwm_limit.h
+11 -9
View File
@@ -46,6 +46,8 @@
#include <stdint.h>
#include <stdbool.h>
__BEGIN_DECLS
/*
* time for the ESCs to initialize
* (this is not actually needed if PWM is sent right after boot)
@@ -56,21 +58,21 @@
*/
#define RAMP_TIME_US 2500000
enum pwm_limit_state {
PWM_LIMIT_STATE_OFF = 0,
PWM_LIMIT_STATE_INIT,
PWM_LIMIT_STATE_RAMP,
PWM_LIMIT_STATE_ON
};
typedef struct {
enum {
LIMIT_STATE_OFF = 0,
LIMIT_STATE_INIT,
LIMIT_STATE_RAMP,
LIMIT_STATE_ON
} state;
enum pwm_limit_state 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);
__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, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit);
__END_DECLS
src/modules/uORB/topics/battery_status.h
+5 -4
View File
@@ -53,9 +53,10 @@
*/
struct battery_status_s {
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
float voltage_v; /**< Battery voltage in volts, filtered */
float current_a; /**< Battery current in amperes, filtered, -1 if unknown */
float discharged_mah; /**< Discharged amount in mAh, filtered, -1 if unknown */
float voltage_v; /**< Battery voltage in volts, 0 if unknown */
float voltage_filtered_v; /**< Battery voltage in volts, filtered, 0 if unknown */
float current_a; /**< Battery current in amperes, -1 if unknown */
float discharged_mah; /**< Discharged amount in mAh, -1 if unknown */
};
/**
@@ -65,4 +66,4 @@ struct battery_status_s {
/* register this as object request broker structure */
ORB_DECLARE(battery_status);
#endif
#endif