Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 03:13:44 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'px4/new_state_machine_drton' into fmuv2_bringup_new_state_machine_drton

Browse Source 
Conflicts:
	src/drivers/blinkm/blinkm.cpp
	src/drivers/px4io/px4io.cpp
	src/modules/commander/state_machine_helper.c
	src/modules/px4iofirmware/protocol.h
	src/modules/px4iofirmware/registers.c
	src/modules/systemlib/systemlib.h
	src/systemcmds/reboot/reboot.c
This commit is contained in:
Julian Oes
2013-08-15 14:04:46 +02:00
parent 50cf1c01b7 561ec495b7
commit 56575eb068
86 changed files with 5851 additions and 3938 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Documentation/dsm_bind.odt
BIN
View File
Binary file not shown.
Documentation/dsm_bind.pdf
BIN
View File
Binary file not shown.
Documentation/flight_mode_state_machine.pdf
BIN
View File
Binary file not shown.
makefiles/config_px4fmu-v1_default.mk
+2 -1
View File
@@ -50,6 +50,7 @@ MODULES += systemcmds/param
MODULES += systemcmds/perf
MODULES += systemcmds/preflight_check
MODULES += systemcmds/pwm
MODULES += systemcmds/esc_calib
MODULES += systemcmds/reboot
MODULES += systemcmds/top
MODULES += systemcmds/tests
@@ -75,7 +76,7 @@ MODULES += examples/flow_position_estimator
#
# Vehicle Control
#
MODULES += modules/segway
#MODULES += modules/segway # XXX needs state machine update
MODULES += modules/fixedwing_backside
MODULES += modules/fixedwing_att_control
MODULES += modules/fixedwing_pos_control
src/drivers/ardrone_interface/ardrone_interface.c
+4 -6
View File
@@ -53,8 +53,10 @@
#include <sys/prctl.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <systemlib/systemlib.h>
@@ -243,16 +245,14 @@ int ardrone_interface_thread_main(int argc, char *argv[])
int led_counter = 0;
/* declare and safely initialize all structs */
struct vehicle_status_s state;
memset(&state, 0, sizeof(state));
struct actuator_controls_s actuator_controls;
memset(&actuator_controls, 0, sizeof(actuator_controls));
struct actuator_armed_s armed;
//XXX is this necessairy?
armed.armed = false;
/* subscribe to attitude, motor setpoints and system state */
int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
printf("[ardrone_interface] Motors initialized - ready.\n");
@@ -322,8 +322,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
} else {
/* MAIN OPERATION MODE */
/* get a local copy of the vehicle state */
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
/* get a local copy of the actuator controls */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
src/drivers/blinkm/blinkm.cpp
+57 -22
View File
@@ -116,6 +116,8 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_gps_position.h>
static const float MAX_CELL_VOLTAGE = 4.3f;
@@ -375,7 +377,9 @@ BlinkM::led()
{
static int vehicle_status_sub_fd;
static int vehicle_control_mode_sub_fd;
static int vehicle_gps_position_sub_fd;
static int actuator_armed_sub_fd;
static int num_of_cells = 0;
static int detected_cells_runcount = 0;
@@ -386,6 +390,8 @@ BlinkM::led()
static int led_interval = 1000;
static int no_data_vehicle_status = 0;
static int no_data_vehicle_control_mode = 0;
static int no_data_actuator_armed = 0;
static int no_data_vehicle_gps_position = 0;
static bool topic_initialized = false;
@@ -398,6 +404,12 @@ BlinkM::led()
vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
orb_set_interval(vehicle_status_sub_fd, 1000);
vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode));
orb_set_interval(vehicle_control_mode_sub_fd, 1000);
actuator_armed_sub_fd = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(actuator_armed_sub_fd, 1000);
vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
orb_set_interval(vehicle_gps_position_sub_fd, 1000);
@@ -452,12 +464,16 @@ BlinkM::led()
if (led_thread_runcount == 15) {
/* obtained data for the first file descriptor */
struct vehicle_status_s vehicle_status_raw;
struct vehicle_control_mode_s vehicle_control_mode;
struct actuator_armed_s actuator_armed;
struct vehicle_gps_position_s vehicle_gps_position_raw;
memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw));
bool new_data_vehicle_status;
bool new_data_vehicle_control_mode;
bool new_data_actuator_armed;
bool new_data_vehicle_gps_position;
orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
@@ -471,6 +487,28 @@ BlinkM::led()
no_data_vehicle_status = 3;
}
orb_check(vehicle_control_mode_sub_fd, &new_data_vehicle_control_mode);
if (new_data_vehicle_control_mode) {
orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub_fd, &vehicle_control_mode);
no_data_vehicle_control_mode = 0;
} else {
no_data_vehicle_control_mode++;
if(no_data_vehicle_control_mode >= 3)
no_data_vehicle_control_mode = 3;
}
orb_check(actuator_armed_sub_fd, &new_data_actuator_armed);
if (new_data_actuator_armed) {
orb_copy(ORB_ID(actuator_armed), actuator_armed_sub_fd, &actuator_armed);
no_data_actuator_armed = 0;
} else {
no_data_actuator_armed++;
if(no_data_actuator_armed >= 3)
no_data_actuator_armed = 3;
}
orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position);
if (new_data_vehicle_gps_position) {
@@ -498,7 +536,7 @@ BlinkM::led()
/* looking for lipo cells that are connected */
printf("<blinkm> checking cells\n");
for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) {
if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break;
if(vehicle_status_raw.battery_voltage < num_of_cells * MAX_CELL_VOLTAGE) break;
}
printf("<blinkm> cells found:%d\n", num_of_cells);
@@ -530,7 +568,7 @@ BlinkM::led()
} else {
/* no battery warnings here */
if(vehicle_status_raw.flag_system_armed == false) {
if(actuator_armed.armed == false) {
/* system not armed */
led_color_1 = LED_RED;
led_color_2 = LED_RED;
@@ -554,27 +592,24 @@ BlinkM::led()
led_color_8 = LED_OFF;
led_blink = LED_BLINK;
/* handle 4th led - flightmode indicator */
switch((int)vehicle_status_raw.flight_mode) {
case VEHICLE_FLIGHT_MODE_MANUAL:
led_color_4 = LED_AMBER;
break;
if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
case VEHICLE_FLIGHT_MODE_STAB:
led_color_4 = LED_YELLOW;
break;
case VEHICLE_FLIGHT_MODE_HOLD:
led_color_4 = LED_BLUE;
break;
case VEHICLE_FLIGHT_MODE_AUTO:
//XXX please check
if (vehicle_control_mode.flag_control_position_enabled)
led_color_4 = LED_GREEN;
break;
else if (vehicle_control_mode.flag_control_velocity_enabled)
led_color_4 = LED_BLUE;
else if (vehicle_control_mode.flag_control_attitude_enabled)
led_color_4 = LED_YELLOW;
else if (vehicle_control_mode.flag_control_manual_enabled)
led_color_4 = LED_AMBER;
else
led_color_4 = LED_OFF;
}
if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
/* handling used sat´s */
/* handling used sat�s */
if(num_of_used_sats >= 7) {
led_color_1 = LED_OFF;
led_color_2 = LED_OFF;
@@ -834,10 +869,10 @@ BlinkM::get_firmware_version(uint8_t version[2])
void blinkm_usage();
void blinkm_usage() {
fprintf(stderr, "missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}\n");
fprintf(stderr, "options:\n");
fprintf(stderr, "\t-b --bus i2cbus (3)\n");
fprintf(stderr, "\t-a --blinkmaddr blinkmaddr (9)\n");
warnx("missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}");
warnx("options:");
warnx("\t-b --bus i2cbus (3)");
warnx("\t-a --blinkmaddr blinkmaddr (9)");
}
int
src/drivers/drv_pwm_output.h
+1 -4
View File
@@ -118,11 +118,8 @@ ORB_DECLARE(output_pwm);
/** start DSM bind */
#define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 7)
/** stop DSM bind */
#define DSM_BIND_STOP _IOC(_PWM_SERVO_BASE, 8)
/** Power up DSM receiver */
#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 9)
#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8)
/** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)
src/drivers/hil/hil.cpp
+1
View File
@@ -75,6 +75,7 @@
#include <systemlib/mixer/mixer.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_outputs.h>
#include <systemlib/err.h>
src/drivers/mkblctrl/mkblctrl.cpp
+8 -7
View File
@@ -75,6 +75,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/esc_status.h>
#include <systemlib/err.h>
@@ -135,7 +136,7 @@ private:
int _current_update_rate;
int _task;
int _t_actuators;
int _t_armed;
int _t_actuator_armed;
unsigned int _motor;
int _px4mode;
int _frametype;
@@ -248,7 +249,7 @@ MK::MK(int bus) :
_update_rate(50),
_task(-1),
_t_actuators(-1),
_t_armed(-1),
_t_actuator_armed(-1),
_t_outputs(0),
_t_actuators_effective(0),
_t_esc_status(0),
@@ -513,8 +514,8 @@ MK::task_main()
/* force a reset of the update rate */
_current_update_rate = 0;
_t_armed = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(_t_armed, 200); /* 5Hz update rate */
_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */
/* advertise the mixed control outputs */
actuator_outputs_s outputs;
@@ -540,7 +541,7 @@ MK::task_main()
pollfd fds[2];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
fds[1].fd = _t_armed;
fds[1].fd = _t_actuator_armed;
fds[1].events = POLLIN;
log("starting");
@@ -654,7 +655,7 @@ MK::task_main()
actuator_armed_s aa;
/* get new value */
orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
/* update PWM servo armed status if armed and not locked down */
mk_servo_arm(aa.armed && !aa.lockdown);
@@ -700,7 +701,7 @@ MK::task_main()
//::close(_t_esc_status);
::close(_t_actuators);
::close(_t_actuators_effective);
::close(_t_armed);
::close(_t_actuator_armed);
/* make sure servos are off */
src/drivers/px4fmu/fmu.cpp
+8 -7
View File
@@ -70,6 +70,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include <systemlib/err.h>
#ifdef HRT_PPM_CHANNEL
@@ -108,7 +109,7 @@ private:
unsigned _current_update_rate;
int _task;
int _t_actuators;
int _t_armed;
int _t_actuator_armed;
orb_advert_t _t_outputs;
orb_advert_t _t_actuators_effective;
unsigned _num_outputs;
@@ -195,7 +196,7 @@ PX4FMU::PX4FMU() :
_current_update_rate(0),
_task(-1),
_t_actuators(-1),
_t_armed(-1),
_t_actuator_armed(-1),
_t_outputs(0),
_t_actuators_effective(0),
_num_outputs(0),
@@ -424,8 +425,8 @@ PX4FMU::task_main()
/* force a reset of the update rate */
_current_update_rate = 0;
_t_armed = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(_t_armed, 200); /* 5Hz update rate */
_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */
/* advertise the mixed control outputs */
actuator_outputs_s outputs;
@@ -444,7 +445,7 @@ PX4FMU::task_main()
pollfd fds[2];
fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
fds[1].fd = _t_armed;
fds[1].fd = _t_actuator_armed;
fds[1].events = POLLIN;
#ifdef HRT_PPM_CHANNEL
@@ -567,7 +568,7 @@ PX4FMU::task_main()
actuator_armed_s aa;
/* get new value */
orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
/* update PWM servo armed status if armed and not locked down */
bool set_armed = aa.armed && !aa.lockdown;
@@ -603,7 +604,7 @@ PX4FMU::task_main()
::close(_t_actuators);
::close(_t_actuators_effective);
::close(_t_armed);
::close(_t_actuator_armed);
/* make sure servos are off */
up_pwm_servo_deinit();
src/drivers/px4io/px4io.cpp
+288 -75
View File
File diff suppressed because it is too large Load Diff
src/drivers/stm32/tone_alarm/tone_alarm.cpp
+25 -4
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (C) 2013 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
@@ -237,6 +237,8 @@ private:
static const unsigned _default_ntunes;
static const uint8_t _note_tab[];
unsigned _default_tune_number; // number of currently playing default tune (0 for none)
const char *_user_tune;
const char *_tune; // current tune string
@@ -452,6 +454,11 @@ const char * const ToneAlarm::_default_tunes[] = {
"O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8"
"O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16"
"O2E2P64",
"MNT75L1O2G", //arming warning
"MBNT100a8", //battery warning slow
"MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"
"a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"
"a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8", //battery warning fast // XXX why is there a break before a repetition
};
const unsigned ToneAlarm::_default_ntunes = sizeof(_default_tunes) / sizeof(_default_tunes[0]);
@@ -467,6 +474,7 @@ extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]);
ToneAlarm::ToneAlarm() :
CDev("tone_alarm", "/dev/tone_alarm"),
_default_tune_number(0),
_user_tune(nullptr),
_tune(nullptr),
_next(nullptr)
@@ -799,8 +807,12 @@ tune_error:
// stop (and potentially restart) the tune
tune_end:
stop_note();
if (_repeat)
if (_repeat) {
start_tune(_tune);
} else {
_tune = nullptr;
_default_tune_number = 0;
}
return;
}
@@ -869,8 +881,17 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg)
_tune = nullptr;
_next = nullptr;
} else {
// play the selected tune
start_tune(_default_tunes[arg - 1]);
/* don't interrupt alarms unless they are repeated */
if (_tune != nullptr && !_repeat) {
/* abort and let the current tune finish */
result = -EBUSY;
} else if (_repeat && _default_tune_number == arg) {
/* requested repeating tune already playing */
} else {
// play the selected tune
_default_tune_number = arg;
start_tune(_default_tunes[arg - 1]);
}
}
} else {
result = -EINVAL;
src/examples/fixedwing_control/main.c
+6 -3
View File
@@ -380,9 +380,10 @@ int fixedwing_control_thread_main(int argc, char *argv[])
/* control */
/* if in auto mode, fly global position setpoint */
if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
#warning fix this
#if 0
if (vstatus.navigation_state == NAVIGATION_STATE_AUTO_ ||
vstatus.navigation_state == NAVIGATION_STATE_STABILIZED) {
/* simple heading control */
control_heading(&global_pos, &global_sp, &att, &att_sp);
@@ -400,6 +401,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
/* set flaps to zero */
actuators.control[4] = 0.0f;
} else if (vstatus.navigation_state == NAVIGATION_STATE_MANUAL) {
/* if in manual mode, decide between attitude stabilization (SAS) and full manual pass-through */
} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
@@ -465,6 +467,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
}
}
}
#endif
/* publish rates */
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
src/examples/flow_position_control/flow_position_control_main.c
+11 -8
View File
@@ -55,7 +55,8 @@
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_local_position.h>
@@ -158,7 +159,8 @@ flow_position_control_thread_main(int argc, char *argv[])
const float time_scale = powf(10.0f,-6.0f);
/* structures */
struct vehicle_status_s vstatus;
struct actuator_armed_s armed;
struct vehicle_control_mode_s control_mode;
struct vehicle_attitude_s att;
struct manual_control_setpoint_s manual;
struct filtered_bottom_flow_s filtered_flow;
@@ -169,7 +171,8 @@ flow_position_control_thread_main(int argc, char *argv[])
/* subscribe to attitude, motor setpoints and system state */
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow));
int vehicle_local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
@@ -258,7 +261,7 @@ flow_position_control_thread_main(int argc, char *argv[])
perf_begin(mc_loop_perf);
/* get a local copy of the vehicle state */
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
/* get a local copy of manual setpoint */
orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual);
/* get a local copy of attitude */
@@ -268,7 +271,7 @@ flow_position_control_thread_main(int argc, char *argv[])
/* get a local copy of local position */
orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos);
if (vstatus.state_machine == SYSTEM_STATE_AUTO)
if (control_mode.flag_control_velocity_enabled)
{
float manual_pitch = manual.pitch / params.rc_scale_pitch; // 0 to 1
float manual_roll = manual.roll / params.rc_scale_roll; // 0 to 1
@@ -490,7 +493,7 @@ flow_position_control_thread_main(int argc, char *argv[])
/* store actual height for speed estimation */
last_local_pos_z = local_pos.z;
speed_sp.thrust_sp = thrust_control;
speed_sp.thrust_sp = thrust_control; //manual.throttle;
speed_sp.timestamp = hrt_absolute_time();
/* publish new speed setpoint */
@@ -527,7 +530,6 @@ flow_position_control_thread_main(int argc, char *argv[])
if(isfinite(manual.throttle))
speed_sp.thrust_sp = manual.throttle;
}
/* measure in what intervals the controller runs */
perf_count(mc_interval_perf);
perf_end(mc_loop_perf);
@@ -576,7 +578,8 @@ flow_position_control_thread_main(int argc, char *argv[])
close(parameter_update_sub);
close(vehicle_attitude_sub);
close(vehicle_local_position_sub);
close(vehicle_status_sub);
close(armed_sub);
close(control_mode_sub);
close(manual_control_setpoint_sub);
close(speed_sp_pub);
src/examples/flow_position_estimator/flow_position_estimator_main.c
+22 -12
View File
@@ -56,7 +56,8 @@
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_local_position.h>
@@ -143,8 +144,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
printf("[flow position estimator] starting\n");
/* rotation matrix for transformation of optical flow speed vectors */
static const int8_t rotM_flow_sensor[3][3] = {{ 0, 1, 0 },
{ -1, 0, 0 },
static const int8_t rotM_flow_sensor[3][3] = {{ 0, -1, 0 },
{ 1, 0, 0 },
{ 0, 0, 1 }}; // 90deg rotated
const float time_scale = powf(10.0f,-6.0f);
static float speed[3] = {0.0f, 0.0f, 0.0f};
@@ -158,8 +159,10 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
static float sonar_lp = 0.0f;
/* subscribe to vehicle status, attitude, sensors and flow*/
struct vehicle_status_s vstatus;
memset(&vstatus, 0, sizeof(vstatus));
struct actuator_armed_s armed;
memset(&armed, 0, sizeof(armed));
struct vehicle_control_mode_s control_mode;
memset(&control_mode, 0, sizeof(control_mode));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
@@ -170,8 +173,11 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
/* subscribe to parameter changes */
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
/* subscribe to vehicle status */
int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
/* subscribe to armed topic */
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
/* subscribe to safety topic */
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
/* subscribe to attitude */
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
@@ -218,6 +224,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
while (!thread_should_exit)
{
if (sensors_ready)
{
/*This runs at the rate of the sensors */
@@ -263,7 +270,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
/* got flow, updating attitude and status as well */
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp);
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
/* vehicle state estimation */
float sonar_new = flow.ground_distance_m;
@@ -273,14 +281,15 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
* -> accept sonar measurements after reaching calibration distance (values between 0.3m and 1.0m for some time)
* -> minimum sonar value 0.3m
*/
if (!vehicle_liftoff)
{
if (vstatus.flag_system_armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f)
if (armed.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f)
vehicle_liftoff = true;
}
else
{
if (!vstatus.flag_system_armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f))
if (!armed.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f))
vehicle_liftoff = false;
}
@@ -347,7 +356,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
}
/* filtering ground distance */
if (!vehicle_liftoff || !vstatus.flag_system_armed)
if (!vehicle_liftoff || !armed.armed)
{
/* not possible to fly */
sonar_valid = false;
@@ -444,7 +453,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
close(vehicle_attitude_setpoint_sub);
close(vehicle_attitude_sub);
close(vehicle_status_sub);
close(armed_sub);
close(control_mode_sub);
close(parameter_update_sub);
close(optical_flow_sub);
src/examples/flow_speed_control/flow_speed_control_main.c
+13 -7
View File
@@ -55,7 +55,8 @@
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_bodyframe_speed_setpoint.h>
@@ -155,7 +156,8 @@ flow_speed_control_thread_main(int argc, char *argv[])
uint32_t counter = 0;
/* structures */
struct vehicle_status_s vstatus;
struct actuator_armed_s armed;
struct vehicle_control_mode_s control_mode;
struct filtered_bottom_flow_s filtered_flow;
struct vehicle_bodyframe_speed_setpoint_s speed_sp;
@@ -164,7 +166,8 @@ flow_speed_control_thread_main(int argc, char *argv[])
/* subscribe to attitude, motor setpoints and system state */
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow));
int vehicle_bodyframe_speed_setpoint_sub = orb_subscribe(ORB_ID(vehicle_bodyframe_speed_setpoint));
@@ -226,14 +229,16 @@ flow_speed_control_thread_main(int argc, char *argv[])
{
perf_begin(mc_loop_perf);
/* get a local copy of the vehicle state */
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
/* get a local copy of the armed topic */
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
/* get a local copy of the control mode */
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
/* get a local copy of filtered bottom flow */
orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow);
/* get a local copy of bodyframe speed setpoint */
orb_copy(ORB_ID(vehicle_bodyframe_speed_setpoint), vehicle_bodyframe_speed_setpoint_sub, &speed_sp);
if (vstatus.state_machine == SYSTEM_STATE_AUTO)
if (control_mode.flag_control_velocity_enabled)
{
/* calc new roll/pitch */
float pitch_body = -(speed_sp.vx - filtered_flow.vx) * params.speed_p;
@@ -350,7 +355,8 @@ flow_speed_control_thread_main(int argc, char *argv[])
close(vehicle_attitude_sub);
close(vehicle_bodyframe_speed_setpoint_sub);
close(filtered_bottom_flow_sub);
close(vehicle_status_sub);
close(armed_sub);
close(control_mode_sub);
close(att_sp_pub);
perf_print_counter(mc_loop_perf);
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+7 -7
View File
@@ -57,7 +57,7 @@
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
@@ -216,8 +216,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
memset(&raw, 0, sizeof(raw));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_status_s state;
memset(&state, 0, sizeof(state));
struct vehicle_control_mode_s control_mode;
memset(&control_mode, 0, sizeof(control_mode));
uint64_t last_data = 0;
uint64_t last_measurement = 0;
@@ -230,8 +230,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* subscribe to param changes */
int sub_params = orb_subscribe(ORB_ID(parameter_update));
/* subscribe to system state*/
int sub_state = orb_subscribe(ORB_ID(vehicle_status));
/* subscribe to control mode*/
int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
/* advertise attitude */
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
@@ -282,9 +282,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* XXX this is seriously bad - should be an emergency */
} else if (ret == 0) {
/* check if we're in HIL - not getting sensor data is fine then */
orb_copy(ORB_ID(vehicle_status), sub_state, &state);
orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);
if (!state.flag_hil_enabled) {
if (!control_mode.flag_system_hil_enabled) {
fprintf(stderr,
"[att ekf] WARNING: Not getting sensors - sensor app running?\n");
}
src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
+7 -7
View File
@@ -35,7 +35,7 @@
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
@@ -547,8 +547,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_status_s state;
memset(&state, 0, sizeof(state));
struct vehicle_control_mode_s control_mode;
memset(&control_mode, 0, sizeof(control_mode));
uint64_t last_data = 0;
uint64_t last_measurement = 0;
@@ -561,8 +561,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* subscribe to param changes */
int sub_params = orb_subscribe(ORB_ID(parameter_update));
/* subscribe to system state*/
int sub_state = orb_subscribe(ORB_ID(vehicle_status));
/* subscribe to control mode */
int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
/* advertise attitude */
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
@@ -612,9 +612,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
/* XXX this is seriously bad - should be an emergency */
} else if (ret == 0) {
/* check if we're in HIL - not getting sensor data is fine then */
orb_copy(ORB_ID(vehicle_status), sub_state, &state);
orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);
if (!state.flag_hil_enabled) {
if (!control_mode.flag_system_hil_enabled) {
fprintf(stderr,
"[att so3_comp] WARNING: Not getting sensors - sensor app running?\n");
}
src/modules/commander/accelerometer_calibration.c → src/modules/commander/accelerometer_calibration.cpp
+52 -41
View File
@@ -33,7 +33,7 @@
****************************************************************************/
/**
* @file accelerometer_calibration.c
* @file accelerometer_calibration.cpp
*
* Implementation of accelerometer calibration.
*
@@ -104,32 +104,43 @@
*/
#include "accelerometer_calibration.h"
#include "commander_helper.h"
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <fcntl.h>
#include <sys/prctl.h>
#include <math.h>
#include <string.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_accel.h>
#include <systemlib/conversions.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <mavlink/mavlink_log.h>
void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd);
int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]);
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]);
int detect_orientation(int mavlink_fd, int sub_sensor_combined);
int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num);
int mat_invert3(float src[3][3], float dst[3][3]);
int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g);
void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd) {
void do_accel_calibration(int mavlink_fd) {
/* announce change */
mavlink_log_info(mavlink_fd, "accel calibration started");
/* set to accel calibration mode */
status->flag_preflight_accel_calibration = true;
state_machine_publish(status_pub, status, mavlink_fd);
/* measure and calculate offsets & scales */
float accel_offs[3];
float accel_scale[3];
int res = do_accel_calibration_mesurements(mavlink_fd, accel_offs, accel_scale);
int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale);
if (res == OK) {
/* measurements complete successfully, set parameters */
@@ -165,24 +176,17 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m
}
mavlink_log_info(mavlink_fd, "accel calibration done");
tune_confirm();
sleep(2);
tune_confirm();
sleep(2);
/* third beep by cal end routine */
tune_positive();
} else {
/* measurements error */
mavlink_log_info(mavlink_fd, "accel calibration aborted");
tune_error();
sleep(2);
tune_negative();
}
/* exit accel calibration mode */
status->flag_preflight_accel_calibration = false;
state_machine_publish(status_pub, status, mavlink_fd);
}
int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) {
int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) {
const int samples_num = 2500;
float accel_ref[6][3];
bool data_collected[6] = { false, false, false, false, false, false };
@@ -229,10 +233,13 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float
sprintf(str, "meas started: %s", orientation_strs[orient]);
mavlink_log_info(mavlink_fd, str);
read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], accel_ref[orient][0], accel_ref[orient][1], accel_ref[orient][2]);
str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient],
(double)accel_ref[orient][0],
(double)accel_ref[orient][1],
(double)accel_ref[orient][2]);
mavlink_log_info(mavlink_fd, str);
data_collected[orient] = true;
tune_confirm();
tune_neutral();
}
close(sensor_combined_sub);
@@ -274,7 +281,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
float accel_err_thr = 5.0f;
/* still time required in us */
int64_t still_time = 2000000;
struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
struct pollfd fds[1];
fds[0].fd = sub_sensor_combined;
fds[0].events = POLLIN;
hrt_abstime t_start = hrt_absolute_time();
/* set timeout to 30s */
@@ -342,29 +351,29 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
}
}
if ( fabs(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
fabs(accel_ema[1]) < accel_err_thr &&
fabs(accel_ema[2]) < accel_err_thr )
if ( fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr )
return 0; // [ g, 0, 0 ]
if ( fabs(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
fabs(accel_ema[1]) < accel_err_thr &&
fabs(accel_ema[2]) < accel_err_thr )
if ( fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr )
return 1; // [ -g, 0, 0 ]
if ( fabs(accel_ema[0]) < accel_err_thr &&
fabs(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
fabs(accel_ema[2]) < accel_err_thr )
if ( fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr )
return 2; // [ 0, g, 0 ]
if ( fabs(accel_ema[0]) < accel_err_thr &&
fabs(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
fabs(accel_ema[2]) < accel_err_thr )
if ( fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr )
return 3; // [ 0, -g, 0 ]
if ( fabs(accel_ema[0]) < accel_err_thr &&
fabs(accel_ema[1]) < accel_err_thr &&
fabs(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr )
if ( fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr )
return 4; // [ 0, 0, g ]
if ( fabs(accel_ema[0]) < accel_err_thr &&
fabs(accel_ema[1]) < accel_err_thr &&
fabs(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr )
if ( fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr )
return 5; // [ 0, 0, -g ]
mavlink_log_info(mavlink_fd, "ERROR: invalid orientation");
@@ -376,7 +385,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
* Read specified number of accelerometer samples, calculate average and dispersion.
*/
int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) {
struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } };
struct pollfd fds[1];
fds[0].fd = sensor_combined_sub;
fds[0].events = POLLIN;
int count = 0;
float accel_sum[3] = { 0.0f, 0.0f, 0.0f };
@@ -404,7 +415,7 @@ int mat_invert3(float src[3][3], float dst[3][3]) {
float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) -
src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
if (det == 0.0)
if (det == 0.0f)
return ERROR; // Singular matrix
dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det;
src/modules/commander/accelerometer_calibration.h
+1 -2
View File
@@ -44,8 +44,7 @@
#define ACCELEROMETER_CALIBRATION_H_
#include <stdint.h>
#include <uORB/topics/vehicle_status.h>
void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd);
void do_accel_calibration(int mavlink_fd);
#endif /* ACCELEROMETER_CALIBRATION_H_ */
src/modules/commander/airspeed_calibration.cpp
+113
View File
@@ -0,0 +1,113 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file airspeed_calibration.cpp
* Airspeed sensor calibration routine
*/
#include "airspeed_calibration.h"
#include "commander_helper.h"
#include <stdio.h>
#include <poll.h>
#include <math.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/differential_pressure.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
void do_airspeed_calibration(int mavlink_fd)
{
/* give directions */
mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still");
const int calibration_count = 2500;
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s diff_pres;
int calibration_counter = 0;
float diff_pres_offset = 0.0f;
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = diff_pres_sub;
fds[0].events = POLLIN;
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
diff_pres_offset += diff_pres.differential_pressure_pa;
calibration_counter++;
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
return;
}
}
diff_pres_offset = diff_pres_offset / calibration_count;
if (isfinite(diff_pres_offset)) {
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, "Setting offs failed!");
}
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
}
//char buf[50];
//sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]);
//mavlink_log_info(mavlink_fd, buf);
mavlink_log_info(mavlink_fd, "airspeed calibration done");
tune_positive();
} else {
mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
}
close(diff_pres_sub);
}
src/modules/commander/airspeed_calibration.h
+46
View File
@@ -0,0 +1,46 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file gyro_calibration.h
* Airspeed sensor calibration routine
*/
#ifndef AIRSPEED_CALIBRATION_H_
#define AIRSPEED_CALIBRATION_H_
#include <stdint.h>
void do_airspeed_calibration(int mavlink_fd);
#endif /* AIRSPEED_CALIBRATION_H_ */
src/modules/commander/baro_calibration.cpp
+54
View File
@@ -0,0 +1,54 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file baro_calibration.cpp
* Barometer calibration routine
*/
#include "baro_calibration.h"
#include <poll.h>
#include <math.h>
#include <fcntl.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_baro.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h>
void do_baro_calibration(int mavlink_fd)
{
// TODO implement this
return;
}
src/modules/commander/baro_calibration.h
+46
View File
@@ -0,0 +1,46 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mag_calibration.h
* Barometer calibration routine
*/
#ifndef BARO_CALIBRATION_H_
#define BARO_CALIBRATION_H_
#include <stdint.h>
void do_baro_calibration(int mavlink_fd);
#endif /* BARO_CALIBRATION_H_ */
src/modules/commander/calibration_routines.c → src/modules/commander/calibration_routines.cpp
+2 -1
View File
@@ -33,7 +33,7 @@
****************************************************************************/
/**
* @file calibration_routines.c
* @file calibration_routines.cpp
* Calibration routines implementations.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
@@ -217,3 +217,4 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
return 0;
}
src/modules/commander/commander.c
-2097
View File
File diff suppressed because it is too large Load Diff
src/modules/commander/commander.cpp
+1640
View File
File diff suppressed because it is too large Load Diff
src/modules/commander/commander_helper.cpp
+219
View File
@@ -0,0 +1,219 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* 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 commander_helper.cpp
* Commander helper functions implementations
*/
#include <stdio.h>
#include <unistd.h>
#include <stdint.h>
#include <stdbool.h>
#include <fcntl.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_tone_alarm.h>
#include <drivers/drv_led.h>
#include "commander_helper.h"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
bool is_multirotor(const struct vehicle_status_s *current_status)
{
return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
(current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
(current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
(current_status->system_type == VEHICLE_TYPE_TRICOPTER));
}
bool is_rotary_wing(const struct vehicle_status_s *current_status)
{
return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER)
|| (current_status->system_type == VEHICLE_TYPE_COAXIAL);
}
static int buzzer;
int buzzer_init()
{
buzzer = open("/dev/tone_alarm", O_WRONLY);
if (buzzer < 0) {
warnx("Buzzer: open fail\n");
return ERROR;
}
return OK;
}
void buzzer_deinit()
{
close(buzzer);
}
void tune_error()
{
ioctl(buzzer, TONE_SET_ALARM, 2);
}
void tune_positive()
{
ioctl(buzzer, TONE_SET_ALARM, 3);
}
void tune_neutral()
{
ioctl(buzzer, TONE_SET_ALARM, 4);
}
void tune_negative()
{
ioctl(buzzer, TONE_SET_ALARM, 5);
}
int tune_arm()
{
return ioctl(buzzer, TONE_SET_ALARM, 12);
}
int tune_critical_bat()
{
return ioctl(buzzer, TONE_SET_ALARM, 14);
}
int tune_low_bat()
{
return ioctl(buzzer, TONE_SET_ALARM, 13);
}
void tune_stop()
{
ioctl(buzzer, TONE_SET_ALARM, 0);
}
static int leds;
int led_init()
{
leds = open(LED_DEVICE_PATH, 0);
if (leds < 0) {
warnx("LED: open fail\n");
return ERROR;
}
if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) {
warnx("LED: ioctl fail\n");
return ERROR;
}
return 0;
}
void led_deinit()
{
close(leds);
}
int led_toggle(int led)
{
static int last_blue = LED_ON;
static int last_amber = LED_ON;
if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON;
if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON;
return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led);
}
int led_on(int led)
{
return ioctl(leds, LED_ON, led);
}
int led_off(int led)
{
return ioctl(leds, LED_OFF, led);
}
float battery_remaining_estimate_voltage(float voltage)
{
float ret = 0;
static param_t bat_volt_empty;
static param_t bat_volt_full;
static param_t bat_n_cells;
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");
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);
}
counter++;
ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty));
/* limit to sane values */
ret = (ret < 0.0f) ? 0.0f : ret;
ret = (ret > 1.0f) ? 1.0f : ret;
return ret;
}
src/modules/commander/commander_helper.h
+80
View File
@@ -0,0 +1,80 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* 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 commander_helper.h
* Commander helper functions definitions
*/
#ifndef COMMANDER_HELPER_H_
#define COMMANDER_HELPER_H_
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_control_mode.h>
bool is_multirotor(const struct vehicle_status_s *current_status);
bool is_rotary_wing(const struct vehicle_status_s *current_status);
int buzzer_init(void);
void buzzer_deinit(void);
void tune_error(void);
void tune_positive(void);
void tune_neutral(void);
void tune_negative(void);
int tune_arm(void);
int tune_critical_bat(void);
int tune_low_bat(void);
void tune_stop(void);
int led_init(void);
void led_deinit(void);
int led_toggle(int led);
int led_on(int led);
int led_off(int led);
/**
* Provides a coarse estimate of remaining battery power.
*
* The estimate is very basic and based on decharging voltage curves.
*
* @return the estimated remaining capacity in 0..1
*/
float battery_remaining_estimate_voltage(float voltage);
#endif /* COMMANDER_HELPER_H_ */
src/modules/commander/commander.h → src/modules/commander/commander_params.c
+14 -18
View File
@@ -1,10 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
* Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -36,23 +33,22 @@
****************************************************************************/
/**
* @file commander.h
* Main system state machine definition.
* @file commander_params.c
*
* Parameters defined by the sensors task.
*
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*
*/
#ifndef COMMANDER_H_
#define COMMANDER_H_
#include <nuttx/config.h>
#include <systemlib/param/param.h>
#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
void tune_confirm(void);
void tune_error(void);
#endif /* COMMANDER_H_ */
PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */
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);
src/modules/commander/gyro_calibration.cpp
+283
View File
@@ -0,0 +1,283 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file gyro_calibration.cpp
* Gyroscope calibration routine
*/
#include "gyro_calibration.h"
#include "commander_helper.h"
#include <stdio.h>
#include <fcntl.h>
#include <poll.h>
#include <math.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_gyro.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
void do_gyro_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still");
const int calibration_count = 5000;
int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
struct sensor_combined_s raw;
int calibration_counter = 0;
float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
/* set offsets to zero */
int fd = open(GYRO_DEVICE_PATH, 0);
struct gyro_scale gscale_null = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null))
warn("WARNING: failed to set scale / offsets for gyro");
close(fd);
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = sub_sensor_combined;
fds[0].events = POLLIN;
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
gyro_offset[0] += raw.gyro_rad_s[0];
gyro_offset[1] += raw.gyro_rad_s[1];
gyro_offset[2] += raw.gyro_rad_s[2];
calibration_counter++;
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
return;
}
}
gyro_offset[0] = gyro_offset[0] / calibration_count;
gyro_offset[1] = gyro_offset[1] / calibration_count;
gyro_offset[2] = gyro_offset[2] / calibration_count;
if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) {
if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))
|| param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))
|| param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) {
mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!");
}
/* set offsets to actual value */
fd = open(GYRO_DEVICE_PATH, 0);
struct gyro_scale gscale = {
gyro_offset[0],
1.0f,
gyro_offset[1],
1.0f,
gyro_offset[2],
1.0f,
};
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
warn("WARNING: failed to set scale / offsets for gyro");
close(fd);
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warnx("WARNING: auto-save of params to storage failed");
mavlink_log_critical(mavlink_fd, "gyro store failed");
// XXX negative tune
return;
}
mavlink_log_info(mavlink_fd, "gyro calibration done");
tune_positive();
/* third beep by cal end routine */
} else {
mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)");
return;
}
/*** --- SCALING --- ***/
mavlink_log_info(mavlink_fd, "offset calibration finished. Rotate for scale 30x");
mavlink_log_info(mavlink_fd, "or do not rotate and wait for 5 seconds to skip.");
warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip.");
unsigned rotations_count = 30;
float gyro_integral = 0.0f;
float baseline_integral = 0.0f;
// XXX change to mag topic
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
float mag_last = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]);
if (mag_last > M_PI_F) mag_last -= 2*M_PI_F;
if (mag_last < -M_PI_F) mag_last += 2*M_PI_F;
uint64_t last_time = hrt_absolute_time();
uint64_t start_time = hrt_absolute_time();
while ((int)fabsf(baseline_integral / (2.0f * M_PI_F)) < rotations_count) {
/* abort this loop if not rotated more than 180 degrees within 5 seconds */
if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f)
&& (hrt_absolute_time() - start_time > 5 * 1e6)) {
mavlink_log_info(mavlink_fd, "gyro scale calibration skipped");
mavlink_log_info(mavlink_fd, "gyro calibration done");
tune_positive();
return;
}
/* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = sub_sensor_combined;
fds[0].events = POLLIN;
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
float dt_ms = (hrt_absolute_time() - last_time) / 1e3f;
last_time = hrt_absolute_time();
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
// XXX this is just a proof of concept and needs world / body
// transformation and more
//math::Vector2f magNav(raw.magnetometer_ga);
// calculate error between estimate and measurement
// apply declination correction for true heading as well.
//float mag = -atan2f(magNav(1),magNav(0));
float mag = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]);
if (mag > M_PI_F) mag -= 2*M_PI_F;
if (mag < -M_PI_F) mag += 2*M_PI_F;
float diff = mag - mag_last;
if (diff > M_PI_F) diff -= 2*M_PI_F;
if (diff < -M_PI_F) diff += 2*M_PI_F;
baseline_integral += diff;
mag_last = mag;
// Jump through some timing scale hoops to avoid
// operating near the 1e6/1e8 max sane resolution of float.
gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f;
warnx("dbg: b: %6.4f, g: %6.4f", baseline_integral, gyro_integral);
// } else if (poll_ret == 0) {
// /* any poll failure for 1s is a reason to abort */
// mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
// return;
}
}
float gyro_scale = baseline_integral / gyro_integral;
float gyro_scales[] = { gyro_scale, gyro_scale, gyro_scale };
warnx("gyro scale: yaw (z): %6.4f", gyro_scale);
mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", gyro_scale);
if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) {
if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scales[0]))
|| param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scales[1]))
|| param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scales[2]))) {
mavlink_log_critical(mavlink_fd, "Setting gyro scale failed!");
}
/* set offsets to actual value */
fd = open(GYRO_DEVICE_PATH, 0);
struct gyro_scale gscale = {
gyro_offset[0],
gyro_scales[0],
gyro_offset[1],
gyro_scales[1],
gyro_offset[2],
gyro_scales[2],
};
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
warn("WARNING: failed to set scale / offsets for gyro");
close(fd);
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
}
// char buf[50];
// sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
// mavlink_log_info(mavlink_fd, buf);
mavlink_log_info(mavlink_fd, "gyro calibration done");
tune_positive();
/* third beep by cal end routine */
} else {
mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)");
}
close(sub_sensor_combined);
}
src/modules/commander/gyro_calibration.h
+46
View File
@@ -0,0 +1,46 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file gyro_calibration.h
* Gyroscope calibration routine
*/
#ifndef GYRO_CALIBRATION_H_
#define GYRO_CALIBRATION_H_
#include <stdint.h>
void do_gyro_calibration(int mavlink_fd);
#endif /* GYRO_CALIBRATION_H_ */
src/modules/commander/mag_calibration.cpp
+280
View File
@@ -0,0 +1,280 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mag_calibration.cpp
* Magnetometer calibration routine
*/
#include "mag_calibration.h"
#include "commander_helper.h"
#include "calibration_routines.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <poll.h>
#include <math.h>
#include <fcntl.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_mag.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
void do_mag_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, "mag calibration starting, hold still");
int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
struct mag_report mag;
/* 45 seconds */
uint64_t calibration_interval = 45 * 1000 * 1000;
/* maximum 2000 values */
const unsigned int calibration_maxcount = 500;
unsigned int calibration_counter = 0;
/* limit update rate to get equally spaced measurements over time (in ms) */
orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
/* erase old calibration */
struct mag_scale mscale_null = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
warn("WARNING: failed to set scale / offsets for mag");
mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag");
}
/* calibrate range */
if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) {
warnx("failed to calibrate scale");
}
close(fd);
/* calibrate offsets */
// uint64_t calibration_start = hrt_absolute_time();
uint64_t axis_deadline = hrt_absolute_time();
uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
const char axislabels[3] = { 'X', 'Y', 'Z'};
int axis_index = -1;
float *x = (float *)malloc(sizeof(float) * calibration_maxcount);
float *y = (float *)malloc(sizeof(float) * calibration_maxcount);
float *z = (float *)malloc(sizeof(float) * calibration_maxcount);
if (x == NULL || y == NULL || z == NULL) {
warnx("mag cal failed: out of memory");
mavlink_log_info(mavlink_fd, "mag cal failed: out of memory");
warnx("x:%p y:%p z:%p\n", x, y, z);
return;
}
while (hrt_absolute_time() < calibration_deadline &&
calibration_counter < calibration_maxcount) {
/* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = sub_mag;
fds[0].events = POLLIN;
/* user guidance */
if (hrt_absolute_time() >= axis_deadline &&
axis_index < 3) {
axis_index++;
char buf[50];
sprintf(buf, "please rotate around %c", axislabels[axis_index]);
mavlink_log_info(mavlink_fd, buf);
tune_neutral();
axis_deadline += calibration_interval / 3;
}
if (!(axis_index < 3)) {
break;
}
// int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time();
// if ((axis_left / 1000) == 0 && axis_left > 0) {
// char buf[50];
// sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]);
// mavlink_log_info(mavlink_fd, buf);
// }
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
x[calibration_counter] = mag.x;
y[calibration_counter] = mag.y;
z[calibration_counter] = mag.z;
/* get min/max values */
// if (mag.x < mag_min[0]) {
// mag_min[0] = mag.x;
// }
// else if (mag.x > mag_max[0]) {
// mag_max[0] = mag.x;
// }
// if (raw.magnetometer_ga[1] < mag_min[1]) {
// mag_min[1] = raw.magnetometer_ga[1];
// }
// else if (raw.magnetometer_ga[1] > mag_max[1]) {
// mag_max[1] = raw.magnetometer_ga[1];
// }
// if (raw.magnetometer_ga[2] < mag_min[2]) {
// mag_min[2] = raw.magnetometer_ga[2];
// }
// else if (raw.magnetometer_ga[2] > mag_max[2]) {
// mag_max[2] = raw.magnetometer_ga[2];
// }
calibration_counter++;
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)");
break;
}
}
float sphere_x;
float sphere_y;
float sphere_z;
float sphere_radius;
sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
free(x);
free(y);
free(z);
if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) {
fd = open(MAG_DEVICE_PATH, 0);
struct mag_scale mscale;
if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale))
warn("WARNING: failed to get scale / offsets for mag");
mscale.x_offset = sphere_x;
mscale.y_offset = sphere_y;
mscale.z_offset = sphere_z;
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
warn("WARNING: failed to set scale / offsets for mag");
close(fd);
/* announce and set new offset */
if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
warnx("Setting X mag offset failed!\n");
}
if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
warnx("Setting Y mag offset failed!\n");
}
if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
warnx("Setting Z mag offset failed!\n");
}
if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
warnx("Setting X mag scale failed!\n");
}
if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
warnx("Setting Y mag scale failed!\n");
}
if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
warnx("Setting Z mag scale failed!\n");
}
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
mavlink_log_info(mavlink_fd, "FAILED storing calibration");
}
warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n",
(double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale,
(double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius);
char buf[52];
sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
(double)mscale.y_offset, (double)mscale.z_offset);
mavlink_log_info(mavlink_fd, buf);
sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
(double)mscale.y_scale, (double)mscale.z_scale);
mavlink_log_info(mavlink_fd, buf);
mavlink_log_info(mavlink_fd, "mag calibration done");
tune_positive();
/* third beep by cal end routine */
} else {
mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
}
close(sub_mag);
}
src/modules/commander/mag_calibration.h
+46
View File
@@ -0,0 +1,46 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mag_calibration.h
* Magnetometer calibration routine
*/
#ifndef MAG_CALIBRATION_H_
#define MAG_CALIBRATION_H_
#include <stdint.h>
void do_mag_calibration(int mavlink_fd);
#endif /* MAG_CALIBRATION_H_ */
src/modules/commander/module.mk
+11 -4
View File
@@ -36,8 +36,15 @@
#
MODULE_COMMAND = commander
SRCS = commander.c \
state_machine_helper.c \
calibration_routines.c \
accelerometer_calibration.c
SRCS = commander.cpp \
commander_params.c \
state_machine_helper.cpp \
commander_helper.cpp \
calibration_routines.cpp \
accelerometer_calibration.cpp \
gyro_calibration.cpp \
mag_calibration.cpp \
baro_calibration.cpp \
rc_calibration.cpp \
airspeed_calibration.cpp
src/modules/commander/px4_custom_mode.h
+18
View File
@@ -0,0 +1,18 @@
/*
* px4_custom_mode.h
*
* Created on: 09.08.2013
* Author: ton
*/
#ifndef PX4_CUSTOM_MODE_H_
#define PX4_CUSTOM_MODE_H_
enum PX4_CUSTOM_MODE {
PX4_CUSTOM_MODE_MANUAL = 1,
PX4_CUSTOM_MODE_SEATBELT,
PX4_CUSTOM_MODE_EASY,
PX4_CUSTOM_MODE_AUTO,
};
#endif /* PX4_CUSTOM_MODE_H_ */
src/modules/commander/rc_calibration.cpp
+83
View File
@@ -0,0 +1,83 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file rc_calibration.cpp
* Remote Control calibration routine
*/
#include "rc_calibration.h"
#include "commander_helper.h"
#include <poll.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
void do_rc_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, "trim calibration starting");
/* XXX fix this */
// if (current_status.rc_signal) {
// mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal.");
// return;
// }
int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
struct manual_control_setpoint_s sp;
orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
/* set parameters */
float p = sp.roll;
param_set(param_find("TRIM_ROLL"), &p);
p = sp.pitch;
param_set(param_find("TRIM_PITCH"), &p);
p = sp.yaw;
param_set(param_find("TRIM_YAW"), &p);
/* store to permanent storage */
/* auto-save */
int save_ret = param_save_default();
if (save_ret != 0) {
mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed");
}
tune_positive();
mavlink_log_info(mavlink_fd, "trim calibration done");
}
src/modules/commander/rc_calibration.h
+46
View File
@@ -0,0 +1,46 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file rc_calibration.h
* Remote Control calibration routine
*/
#ifndef RC_CALIBRATION_H_
#define RC_CALIBRATION_H_
#include <stdint.h>
void do_rc_calibration(int mavlink_fd);
#endif /* RC_CALIBRATION_H_ */
src/modules/commander/state_machine_helper.c
-757
View File
File diff suppressed because it is too large Load Diff
src/modules/commander/state_machine_helper.cpp
+695
View File
File diff suppressed because it is too large Load Diff
src/modules/commander/state_machine_helper.h
+18 -150
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
*
@@ -46,164 +46,32 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/vehicle_control_mode.h>
/**
* Switch to new state with no checking.
*
* do_state_update: this is the functions that all other functions have to call in order to update the state.
* the function does not question the state change, this must be done before
* The function performs actions that are connected with the new state (buzzer, reboot, ...)
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*
* @return 0 (macro OK) or 1 on error (macro ERROR)
*/
int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state);
typedef enum {
TRANSITION_DENIED = -1,
TRANSITION_NOT_CHANGED = 0,
TRANSITION_CHANGED
/* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR */
// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
} transition_result_t;
// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
arming_state_t new_arming_state, struct actuator_armed_s *armed);
// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
bool check_arming_state_changed();
/**
* Handle state machine if got position fix
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*/
void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);
/**
* Handle state machine if position fix lost
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*/
void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
bool check_main_state_changed();
/**
* Handle state machine if user wants to arm
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*/
void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
/**
* Handle state machine if user wants to disarm
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*/
void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
/**
* Handle state machine if mode switch is manual
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*/
void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
/**
* Handle state machine if mode switch is stabilized
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*/
void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
/**
* Handle state machine if mode switch is guided
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*/
void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
/**
* Handle state machine if mode switch is auto
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*/
void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
/**
* Publish current state information
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*/
void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
/*
* Functions that handle incoming requests to change the state machine or a parameter (probably from the mavlink app).
* If the request is obeyed the functions return 0
*
*/
/**
* Handles *incoming request* to switch to a specific state, if state change is successful returns 0
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*/
uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode);
/**
* Handles *incoming request* to switch to a specific custom state, if state change is successful returns 0
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*/
uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode);
/**
* Always switches to critical mode under any circumstances.
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*/
void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
/**
* Switches to emergency if required.
*
* @param status_pub file descriptor for state update topic publication
* @param current_status pointer to the current state machine to operate on
* @param mavlink_fd file descriptor for MAVLink statustext messages
*/
void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
/**
* Publish the armed state depending on the current system state
*
* @param current_status the current system status
*/
void publish_armed_status(const struct vehicle_status_s *current_status);
transition_result_t navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode);
bool check_navigation_state_changed();
int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd);
#endif /* STATE_MACHINE_HELPER_H_ */
src/modules/fixedwing_att_control/fixedwing_att_control_att.c
+2 -2
View File
@@ -142,7 +142,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
}
/* Roll (P) */
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0);
rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0, NULL, NULL, NULL);
/* Pitch (P) */
@@ -152,7 +152,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
/* set pitch plus feedforward roll compensation */
rates_sp->pitch = pid_calculate(&pitch_controller,
att_sp->pitch_body + pitch_sp_rollcompensation,
att->pitch, 0, 0);
att->pitch, 0, 0, NULL, NULL, NULL);
/* Yaw (from coordinated turn constraint or lateral force) */
rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll))
src/modules/fixedwing_att_control/fixedwing_att_control_main.c
+82 -81
View File
@@ -186,87 +186,88 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
/* control */
if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
/* attitude control */
fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
/* angular rate control */
fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
/* pass through throttle */
actuators.control[3] = att_sp.thrust;
/* set flaps to zero */
actuators.control[4] = 0.0f;
} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
/* if the RC signal is lost, try to stay level and go slowly back down to ground */
if (vstatus.rc_signal_lost) {
/* put plane into loiter */
att_sp.roll_body = 0.3f;
att_sp.pitch_body = 0.0f;
/* limit throttle to 60 % of last value if sane */
if (isfinite(manual_sp.throttle) &&
(manual_sp.throttle >= 0.0f) &&
(manual_sp.throttle <= 1.0f)) {
att_sp.thrust = 0.6f * manual_sp.throttle;
} else {
att_sp.thrust = 0.0f;
}
att_sp.yaw_body = 0;
// XXX disable yaw control, loiter
} else {
att_sp.roll_body = manual_sp.roll;
att_sp.pitch_body = manual_sp.pitch;
att_sp.yaw_body = 0;
att_sp.thrust = manual_sp.throttle;
}
att_sp.timestamp = hrt_absolute_time();
/* attitude control */
fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
/* angular rate control */
fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
/* pass through throttle */
actuators.control[3] = att_sp.thrust;
/* pass through flaps */
if (isfinite(manual_sp.flaps)) {
actuators.control[4] = manual_sp.flaps;
} else {
actuators.control[4] = 0.0f;
}
} else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
/* directly pass through values */
actuators.control[0] = manual_sp.roll;
/* positive pitch means negative actuator -> pull up */
actuators.control[1] = manual_sp.pitch;
actuators.control[2] = manual_sp.yaw;
actuators.control[3] = manual_sp.throttle;
if (isfinite(manual_sp.flaps)) {
actuators.control[4] = manual_sp.flaps;
} else {
actuators.control[4] = 0.0f;
}
}
}
#warning fix this
// if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
// vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
// /* attitude control */
// fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
//
// /* angular rate control */
// fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
//
// /* pass through throttle */
// actuators.control[3] = att_sp.thrust;
//
// /* set flaps to zero */
// actuators.control[4] = 0.0f;
//
// } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
// if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
//
// /* if the RC signal is lost, try to stay level and go slowly back down to ground */
// if (vstatus.rc_signal_lost) {
//
// /* put plane into loiter */
// att_sp.roll_body = 0.3f;
// att_sp.pitch_body = 0.0f;
//
// /* limit throttle to 60 % of last value if sane */
// if (isfinite(manual_sp.throttle) &&
// (manual_sp.throttle >= 0.0f) &&
// (manual_sp.throttle <= 1.0f)) {
// att_sp.thrust = 0.6f * manual_sp.throttle;
//
// } else {
// att_sp.thrust = 0.0f;
// }
//
// att_sp.yaw_body = 0;
//
// // XXX disable yaw control, loiter
//
// } else {
//
// att_sp.roll_body = manual_sp.roll;
// att_sp.pitch_body = manual_sp.pitch;
// att_sp.yaw_body = 0;
// att_sp.thrust = manual_sp.throttle;
// }
//
// att_sp.timestamp = hrt_absolute_time();
//
// /* attitude control */
// fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
//
// /* angular rate control */
// fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
//
// /* pass through throttle */
// actuators.control[3] = att_sp.thrust;
//
// /* pass through flaps */
// if (isfinite(manual_sp.flaps)) {
// actuators.control[4] = manual_sp.flaps;
//
// } else {
// actuators.control[4] = 0.0f;
// }
//
// } else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
// /* directly pass through values */
// actuators.control[0] = manual_sp.roll;
// /* positive pitch means negative actuator -> pull up */
// actuators.control[1] = manual_sp.pitch;
// actuators.control[2] = manual_sp.yaw;
// actuators.control[3] = manual_sp.throttle;
//
// if (isfinite(manual_sp.flaps)) {
// actuators.control[4] = manual_sp.flaps;
//
// } else {
// actuators.control[4] = 0.0f;
// }
// }
// }
/* publish rates */
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
+3 -3
View File
@@ -196,11 +196,11 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
/* roll rate (PI) */
actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT, NULL, NULL, NULL);
/* pitch rate (PI) */
actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT, NULL, NULL, NULL);
/* yaw rate (PI) */
actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT, NULL, NULL, NULL);
counter++;
src/modules/fixedwing_backside/fixedwing.cpp
+31 -22
View File
@@ -39,6 +39,8 @@
#include "fixedwing.hpp"
#if 0
namespace control
{
@@ -155,8 +157,9 @@ void BlockMultiModeBacksideAutopilot::update()
for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++)
_actuators.control[i] = 0.0f;
#warning this if is incomplete, should be based on flags
// only update guidance in auto mode
if (_status.state_machine == SYSTEM_STATE_AUTO) {
if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) {
// update guidance
_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
}
@@ -165,9 +168,10 @@ void BlockMultiModeBacksideAutopilot::update()
// once the system switches from manual or auto to stabilized
// the setpoint should update to loitering around this position
#warning should be base on flags
// handle autopilot modes
if (_status.state_machine == SYSTEM_STATE_AUTO ||
_status.state_machine == SYSTEM_STATE_STABILIZED) {
if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
_status.navigation_state == NAVIGATION_STATE_SEATBELT) {
// update guidance
_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
@@ -219,21 +223,23 @@ void BlockMultiModeBacksideAutopilot::update()
// This is not a hack, but a design choice.
/* do not limit in HIL */
if (!_status.flag_hil_enabled) {
/* limit to value of manual throttle */
_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
_actuators.control[CH_THR] : _manual.throttle;
}
#warning fix this
// if (!_status.flag_hil_enabled) {
// /* limit to value of manual throttle */
// _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
// _actuators.control[CH_THR] : _manual.throttle;
// }
} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
} else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) {
if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
_actuators.control[CH_AIL] = _manual.roll;
_actuators.control[CH_ELV] = _manual.pitch;
_actuators.control[CH_RDR] = _manual.yaw;
_actuators.control[CH_THR] = _manual.throttle;
} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
#warning fix here too
// if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
// _actuators.control[CH_AIL] = _manual.roll;
// _actuators.control[CH_ELV] = _manual.pitch;
// _actuators.control[CH_RDR] = _manual.yaw;
// _actuators.control[CH_THR] = _manual.throttle;
//
// } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
// calculate velocity, XXX should be airspeed, but using ground speed for now
// for the purpose of control we will limit the velocity feedback between
@@ -284,12 +290,14 @@ void BlockMultiModeBacksideAutopilot::update()
// This is not a hack, but a design choice.
/* do not limit in HIL */
if (!_status.flag_hil_enabled) {
/* limit to value of manual throttle */
_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
_actuators.control[CH_THR] : _manual.throttle;
}
}
#warning fix this
// if (!_status.flag_hil_enabled) {
// /* limit to value of manual throttle */
// _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
// _actuators.control[CH_THR] : _manual.throttle;
// }
#warning fix this
// }
// body rates controller, disabled for now
else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) {
@@ -322,3 +330,4 @@ BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot()
} // namespace control
#endif
src/modules/fixedwing_backside/fixedwing_backside_main.cpp
+4 -2
View File
@@ -151,12 +151,14 @@ int control_demo_thread_main(int argc, char *argv[])
using namespace control;
fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB");
#warning fix this
// fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB");
thread_running = true;
while (!thread_should_exit) {
autopilot.update();
#warning fix this
// autopilot.update();
}
warnx("exiting.");
src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
+5 -5
View File
@@ -271,7 +271,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit
pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim);
pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim);
pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD); //TODO: remove hardcoded value
pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD_F); //TODO: remove hardcoded value
}
/* only run controller if attitude changed */
@@ -319,7 +319,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
// XXX what is xtrack_err.past_end?
if (distance_res == OK /*&& !xtrack_err.past_end*/) {
float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance
float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f, NULL, NULL, NULL); //p.xtrack_p * xtrack_err.distance
float psi_c = psi_track + delta_psi_c;
float psi_e = psi_c - att.yaw;
@@ -336,7 +336,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
}
/* calculate roll setpoint, do this artificially around zero */
float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f);
float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f, NULL, NULL, NULL);
float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following
float psi_rate_c = delta_psi_rate_c + psi_rate_track;
@@ -359,7 +359,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g
attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT);
attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT, NULL, NULL, NULL);
if (verbose) {
printf("psi_rate_c %.4f ", (double)psi_rate_c);
@@ -379,7 +379,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
if (pos_updated) {
//TODO: take care of relative vs. ab. altitude
attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f, NULL, NULL, NULL);
}
src/modules/gpio_led/gpio_led.c
+11 -4
View File
@@ -51,6 +51,7 @@
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <poll.h>
#include <drivers/drv_gpio.h>
#include <modules/px4iofirmware/protocol.h>
@@ -62,6 +63,8 @@ struct gpio_led_s {
int pin;
struct vehicle_status_s status;
int vehicle_status_sub;
struct actuator_armed_s armed;
int actuator_armed_sub;
bool led_state;
int counter;
};
@@ -227,10 +230,15 @@ void gpio_led_cycle(FAR void *arg)
if (status_updated)
orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status);
orb_check(priv->vehicle_status_sub, &status_updated);
if (status_updated)
orb_copy(ORB_ID(actuator_armed), priv->actuator_armed_sub, &priv->armed);
/* select pattern for current status */
int pattern = 0;
if (priv->status.flag_system_armed) {
if (priv->armed.armed) {
if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
pattern = 0x3f; // ****** solid (armed)
@@ -239,11 +247,10 @@ void gpio_led_cycle(FAR void *arg)
}
} else {
if (priv->status.state_machine == SYSTEM_STATE_PREFLIGHT) {
if (priv->armed.ready_to_arm) {
pattern = 0x00; // ______ off (disarmed, preflight check)
} else if (priv->status.state_machine == SYSTEM_STATE_STANDBY &&
priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
} else if (priv->armed.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
pattern = 0x38; // ***___ slow blink (disarmed, ready)
} else {
src/modules/mavlink/mavlink.c
+59 -87
View File
@@ -64,6 +64,7 @@
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <mavlink/mavlink_log.h>
#include <commander/px4_custom_mode.h>
#include "waypoints.h"
#include "orb_topics.h"
@@ -181,102 +182,68 @@ set_hil_on_off(bool hil_enabled)
}
void
get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
{
/* reset MAVLink mode bitfield */
*mavlink_mode = 0;
*mavlink_base_mode = 0;
*mavlink_custom_mode = 0;
/* set mode flags independent of system state */
/**
* Set mode flags
**/
/* HIL */
if (v_status.flag_hil_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
if (v_status.hil_state == HIL_STATE_ON) {
*mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
}
/* manual input */
if (v_status.flag_control_manual_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
/* arming state */
if (v_status.arming_state == ARMING_STATE_ARMED
|| v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
}
/* attitude or rate control */
if (v_status.flag_control_attitude_enabled ||
v_status.flag_control_rates_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
/* main state */
*mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
if (v_status.main_state == MAIN_STATE_MANUAL) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
*mavlink_custom_mode = PX4_CUSTOM_MODE_MANUAL;
} else if (v_status.main_state == MAIN_STATE_SEATBELT) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
*mavlink_custom_mode = PX4_CUSTOM_MODE_SEATBELT;
} else if (v_status.main_state == MAIN_STATE_EASY) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
*mavlink_custom_mode = PX4_CUSTOM_MODE_EASY;
} else if (v_status.main_state == MAIN_STATE_AUTO) {
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
*mavlink_custom_mode = PX4_CUSTOM_MODE_AUTO;
}
/* vector control */
if (v_status.flag_control_velocity_enabled ||
v_status.flag_control_position_enabled) {
*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
}
/**
* Set mavlink state
**/
/* autonomous mode */
if (v_status.state_machine == SYSTEM_STATE_AUTO) {
*mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
}
/* set arming state */
if (armed.armed) {
*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
} else {
*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
}
switch (v_status.state_machine) {
case SYSTEM_STATE_PREFLIGHT:
if (v_status.flag_preflight_gyro_calibration ||
v_status.flag_preflight_mag_calibration ||
v_status.flag_preflight_accel_calibration) {
*mavlink_state = MAV_STATE_CALIBRATING;
} else {
*mavlink_state = MAV_STATE_UNINIT;
}
break;
case SYSTEM_STATE_STANDBY:
/* set calibration state */
if (v_status.preflight_calibration) {
*mavlink_state = MAV_STATE_CALIBRATING;
} else if (v_status.system_emergency) {
*mavlink_state = MAV_STATE_EMERGENCY;
} else if (v_status.arming_state == ARMING_STATE_INIT
|| v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE
|| v_status.arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review
*mavlink_state = MAV_STATE_UNINIT;
} else if (v_status.arming_state == ARMING_STATE_ARMED) {
*mavlink_state = MAV_STATE_ACTIVE;
} else if (v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
*mavlink_state = MAV_STATE_CRITICAL;
} else if (v_status.arming_state == ARMING_STATE_STANDBY) {
*mavlink_state = MAV_STATE_STANDBY;
break;
case SYSTEM_STATE_GROUND_READY:
*mavlink_state = MAV_STATE_ACTIVE;
break;
case SYSTEM_STATE_MANUAL:
*mavlink_state = MAV_STATE_ACTIVE;
break;
case SYSTEM_STATE_STABILIZED:
*mavlink_state = MAV_STATE_ACTIVE;
break;
case SYSTEM_STATE_AUTO:
*mavlink_state = MAV_STATE_ACTIVE;
break;
case SYSTEM_STATE_MISSION_ABORT:
*mavlink_state = MAV_STATE_EMERGENCY;
break;
case SYSTEM_STATE_EMCY_LANDING:
*mavlink_state = MAV_STATE_EMERGENCY;
break;
case SYSTEM_STATE_EMCY_CUTOFF:
*mavlink_state = MAV_STATE_EMERGENCY;
break;
case SYSTEM_STATE_GROUND_ERROR:
*mavlink_state = MAV_STATE_EMERGENCY;
break;
case SYSTEM_STATE_REBOOT:
} else if (v_status.arming_state == ARMING_STATE_REBOOT) {
*mavlink_state = MAV_STATE_POWEROFF;
break;
} else {
warnx("Unknown mavlink state");
*mavlink_state = MAV_STATE_CRITICAL;
}
}
@@ -568,6 +535,7 @@ int mavlink_thread_main(int argc, char *argv[])
default:
usage();
break;
}
}
@@ -674,14 +642,18 @@ int mavlink_thread_main(int argc, char *argv[])
/* translate the current system state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_mode = 0;
get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
uint8_t mavlink_base_mode = 0;
uint32_t mavlink_custom_mode = 0;
get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
/* send heartbeat */
mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state);
/* switch HIL mode if required */
set_hil_on_off(v_status.flag_hil_enabled);
if (v_status.hil_state == HIL_STATE_ON)
set_hil_on_off(true);
else if (v_status.hil_state == HIL_STATE_OFF)
set_hil_on_off(false);
/* send status (values already copied in the section above) */
mavlink_msg_sys_status_send(chan,
@@ -689,8 +661,8 @@ int mavlink_thread_main(int argc, char *argv[])
v_status.onboard_control_sensors_enabled,
v_status.onboard_control_sensors_health,
v_status.load,
v_status.voltage_battery * 1000.0f,
v_status.current_battery * 1000.0f,
v_status.battery_voltage * 1000.0f,
v_status.battery_current * 1000.0f,
v_status.battery_remaining,
v_status.drop_rate_comm,
v_status.errors_comm,
src/modules/mavlink/orb_listener.c
+17 -12
View File
@@ -272,19 +272,23 @@ l_vehicle_status(const struct listener *l)
orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
/* enable or disable HIL */
set_hil_on_off(v_status.flag_hil_enabled);
if (v_status.hil_state == HIL_STATE_ON)
set_hil_on_off(true);
else if (v_status.hil_state == HIL_STATE_OFF)
set_hil_on_off(false);
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_mode = 0;
get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
uint8_t mavlink_base_mode = 0;
uint32_t mavlink_custom_mode = 0;
get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
/* send heartbeat */
mavlink_msg_heartbeat_send(chan,
mavlink_system.type,
MAV_AUTOPILOT_PX4,
mavlink_mode,
v_status.state_machine,
mavlink_base_mode,
mavlink_custom_mode,
mavlink_state);
}
@@ -470,8 +474,9 @@ l_actuator_outputs(const struct listener *l)
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state = 0;
uint8_t mavlink_mode = 0;
get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
uint8_t mavlink_base_mode = 0;
uint32_t mavlink_custom_mode = 0;
get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
/* HIL message as per MAVLink spec */
@@ -488,7 +493,7 @@ l_actuator_outputs(const struct listener *l)
-1,
-1,
-1,
mavlink_mode,
mavlink_base_mode,
0);
} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
@@ -502,7 +507,7 @@ l_actuator_outputs(const struct listener *l)
((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
-1,
-1,
mavlink_mode,
mavlink_base_mode,
0);
} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
@@ -516,7 +521,7 @@ l_actuator_outputs(const struct listener *l)
((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f,
((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f,
mavlink_mode,
mavlink_base_mode,
0);
} else {
@@ -530,7 +535,7 @@ l_actuator_outputs(const struct listener *l)
(act_outputs.output[5] - 1500.0f) / 500.0f,
(act_outputs.output[6] - 1500.0f) / 500.0f,
(act_outputs.output[7] - 1500.0f) / 500.0f,
mavlink_mode,
mavlink_base_mode,
0);
}
}
@@ -673,7 +678,7 @@ uorb_receive_thread(void *arg)
/* handle the poll result */
if (poll_ret == 0) {
mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s");
/* silent */
} else if (poll_ret < 0) {
mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data");

Some files were not shown because too many files have changed in this diff Show More