mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-20 20:03:54 +08:00
Merged parameter changes
This commit is contained in:
+3
-1
@@ -25,7 +25,9 @@ ROMFS_FSSPEC := $(SRCROOT)/scripts/rcS~init.d/rcS \
|
||||
$(SRCROOT)/mixers/FMU_delta.mix~mixers/FMU_delta.mix \
|
||||
$(SRCROOT)/mixers/FMU_AERT.mix~mixers/FMU_AERT.mix \
|
||||
$(SRCROOT)/mixers/FMU_AET.mix~mixers/FMU_AET.mix \
|
||||
$(SRCROOT)/mixers/FMU_RET.mix~mixers/FMU_ERT.mix
|
||||
$(SRCROOT)/mixers/FMU_RET.mix~mixers/FMU_ERT.mix \
|
||||
$(SRCROOT)/mixers/FMU_quad_x.mix~mixers/FMU_quad_x.mix \
|
||||
$(SRCROOT)/mixers/FMU_quad_+.mix~mixers/FMU_quad_+.mix
|
||||
|
||||
#
|
||||
# Add the PX4IO firmware to the spec if someone has dropped it into the
|
||||
|
||||
@@ -0,0 +1,7 @@
|
||||
Multirotor mixer for PX4FMU
|
||||
===========================
|
||||
|
||||
This file defines a single mixer for a quadrotor in the + configuration, with 10% contribution from
|
||||
roll and pitch and 20% contribution from yaw and no deadband.
|
||||
|
||||
R: 4+ 1000 1000 2000 0
|
||||
@@ -37,7 +37,7 @@
|
||||
|
||||
APPNAME = commander
|
||||
PRIORITY = SCHED_PRIORITY_MAX - 30
|
||||
STACKSIZE = 4096
|
||||
STACKSIZE = 2048
|
||||
|
||||
INCLUDES = $(TOPDIR)/../mavlink/include/mavlink
|
||||
|
||||
|
||||
+92
-14
@@ -69,7 +69,8 @@
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
|
||||
#include <arch/board/up_cpuload.h>
|
||||
@@ -101,9 +102,9 @@ static orb_advert_t stat_pub;
|
||||
static uint16_t nofix_counter = 0;
|
||||
static uint16_t gotfix_counter = 0;
|
||||
|
||||
static void do_gyro_calibration(int status_pub, struct vehicle_status_s *current_status);
|
||||
static void do_mag_calibration(int status_pub, struct vehicle_status_s *current_status);
|
||||
static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd);
|
||||
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||
static bool thread_running = false; /**< Deamon status flag */
|
||||
static int deamon_task; /**< Handle of deamon task / thread */
|
||||
|
||||
/* pthread loops */
|
||||
static void *command_handling_loop(void *arg);
|
||||
@@ -111,9 +112,30 @@ static void *command_handling_loop(void *arg);
|
||||
|
||||
__EXPORT int commander_main(int argc, char *argv[]);
|
||||
|
||||
#ifdef CONFIG_TONE_ALARM
|
||||
/**
|
||||
* Mainloop of commander.
|
||||
*/
|
||||
int commander_thread_main(int argc, char *argv[]);
|
||||
|
||||
static int buzzer_init(void);
|
||||
static void buzzer_deinit(void);
|
||||
static int led_init(void);
|
||||
static void led_deinit(void);
|
||||
static int led_toggle(int led);
|
||||
static int led_on(int led);
|
||||
static int led_off(int led);
|
||||
static void do_gyro_calibration(int status_pub, struct vehicle_status_s *current_status);
|
||||
static void do_mag_calibration(int status_pub, struct vehicle_status_s *current_status);
|
||||
static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd);
|
||||
|
||||
int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state);
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Print the correct usage.
|
||||
*/
|
||||
static void usage(const char *reason);
|
||||
|
||||
static int buzzer_init()
|
||||
{
|
||||
@@ -131,13 +153,7 @@ static void buzzer_deinit()
|
||||
{
|
||||
close(buzzer);
|
||||
}
|
||||
#endif
|
||||
|
||||
static int led_init(void);
|
||||
static void led_deinit(void);
|
||||
static int led_toggle(int led);
|
||||
static int led_on(int led);
|
||||
static int led_off(int led);
|
||||
|
||||
static int led_init()
|
||||
{
|
||||
@@ -581,6 +597,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
|
||||
}
|
||||
break;
|
||||
|
||||
/*
|
||||
* do not report an error for commands that are
|
||||
* handled directly by MAVLink.
|
||||
*/
|
||||
case MAV_CMD_PREFLIGHT_STORAGE:
|
||||
break;
|
||||
|
||||
default: {
|
||||
mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported command");
|
||||
result = MAV_RESULT_UNSUPPORTED;
|
||||
@@ -705,6 +728,9 @@ enum BAT_CHEM {
|
||||
*/
|
||||
float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage);
|
||||
|
||||
PARAM_DEFINE_FLOAT(BAT_VOLT_EMPTY, 3.2f);
|
||||
PARAM_DEFINE_FLOAT(BAT_VOLT_FULL, 4.05f);
|
||||
|
||||
float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage)
|
||||
{
|
||||
float ret = 0;
|
||||
@@ -721,11 +747,61 @@ float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage
|
||||
return ret;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: commander
|
||||
****************************************************************************/
|
||||
static void
|
||||
usage(const char *reason)
|
||||
{
|
||||
if (reason)
|
||||
fprintf(stderr, "%s\n", reason);
|
||||
fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
/**
|
||||
* The deamon app only briefly exists to start
|
||||
* the background job. The stack size assigned in the
|
||||
* Makefile does only apply to this management task.
|
||||
*
|
||||
* The actual stack size should be set in the call
|
||||
* to task_create().
|
||||
*/
|
||||
int commander_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 1)
|
||||
usage("missing command");
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
if (thread_running) {
|
||||
printf("commander already running\n");
|
||||
/* this is not an error */
|
||||
exit(0);
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
deamon_task = task_create("commander", SCHED_PRIORITY_MAX - 50, 4096, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
thread_running = true;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
thread_should_exit = true;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running) {
|
||||
printf("\tcommander is running\n");
|
||||
} else {
|
||||
printf("\tcommander not started\n");
|
||||
}
|
||||
exit(0);
|
||||
}
|
||||
|
||||
usage("unrecognized command");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
int commander_thread_main(int argc, char *argv[])
|
||||
{
|
||||
/* not yet initialized */
|
||||
commander_initialized = false;
|
||||
@@ -1108,6 +1184,8 @@ int commander_main(int argc, char *argv[])
|
||||
led_deinit();
|
||||
buzzer_deinit();
|
||||
|
||||
thread_running = false;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
@@ -42,8 +42,8 @@
|
||||
#include <stdio.h>
|
||||
|
||||
static bool thread_should_exit = false; /**< Deamon exit flag */
|
||||
static bool thread_running = false; /**< Deamon status flag */
|
||||
static int deamon_task; /**< Handle of deamon task / thread */
|
||||
static bool thread_running = false; /**< Deamon status flag */
|
||||
static int deamon_task; /**< Handle of deamon task / thread */
|
||||
|
||||
/**
|
||||
* Deamon management function.
|
||||
@@ -94,7 +94,7 @@ usage(const char *reason)
|
||||
*/
|
||||
int px4_deamon_app_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 1)
|
||||
if (argc < 1)
|
||||
usage("missing command");
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
@@ -60,10 +60,7 @@
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/fixedwing_control.h>
|
||||
|
||||
#ifndef F_M_PI
|
||||
#define F_M_PI ((float)M_PI)
|
||||
#endif
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
__EXPORT int fixedwing_control_main(int argc, char *argv[]);
|
||||
|
||||
@@ -291,6 +288,18 @@ static float pid(float error, float error_deriv, uint16_t dt, float scale, float
|
||||
return output;
|
||||
}
|
||||
|
||||
PARAM_DEFINE_FLOAT(FW_ATT_ROLL_P, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_ATT_ROLL_I, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_ATT_ROLL_D, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_ATT_ROLL_AWU, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_ATT_ROLL_LIM, 0.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(FW_ATT_PITCH_P, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_ATT_PITCH_I, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_ATT_PITCH_D, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_ATT_PITCH_AWU, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(FW_ATT_PITCH_LIM, 0.0f);
|
||||
|
||||
/**
|
||||
* Load parameters from global storage.
|
||||
*
|
||||
@@ -299,7 +308,6 @@ static float pid(float error, float error_deriv, uint16_t dt, float scale, float
|
||||
* Fetches the current parameters from the global parameter storage and writes them
|
||||
* to the plane_data structure
|
||||
*/
|
||||
|
||||
static void get_parameters(plane_data_t * plane_data)
|
||||
{
|
||||
plane_data->Kp_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P];
|
||||
@@ -395,10 +403,10 @@ static void calc_rotation_matrix(float roll, float pitch, float yaw, float x, fl
|
||||
*/
|
||||
static float calc_bearing()
|
||||
{
|
||||
float bearing = F_M_PI/2.0f + (float)atan2(-(plane_data.wp_y - plane_data.lat), (plane_data.wp_x - plane_data.lon));
|
||||
float bearing = M_PI_F/2.0f + (float)atan2(-(plane_data.wp_y - plane_data.lat), (plane_data.wp_x - plane_data.lon));
|
||||
|
||||
if (bearing < 0.0f) {
|
||||
bearing += 2*F_M_PI;
|
||||
bearing += 2*M_PI_F;
|
||||
}
|
||||
|
||||
return bearing;
|
||||
@@ -543,11 +551,11 @@ static float calc_roll_setpoint()
|
||||
} else {
|
||||
setpoint = calc_bearing() - plane_data.yaw;
|
||||
|
||||
if (setpoint < (-35.0f/180.0f)*F_M_PI)
|
||||
return (-35.0f/180.0f)*F_M_PI;
|
||||
if (setpoint < (-35.0f/180.0f)*M_PI_F)
|
||||
return (-35.0f/180.0f)*M_PI_F;
|
||||
|
||||
if (setpoint > (35/180.0f)*F_M_PI)
|
||||
return (-35.0f/180.0f)*F_M_PI;
|
||||
if (setpoint > (35/180.0f)*M_PI_F)
|
||||
return (-35.0f/180.0f)*M_PI_F;
|
||||
}
|
||||
|
||||
return setpoint;
|
||||
@@ -568,16 +576,16 @@ static float calc_pitch_setpoint()
|
||||
float setpoint = 0.0f;
|
||||
|
||||
if (plane_data.mode == TAKEOFF) {
|
||||
setpoint = ((35/180.0f)*F_M_PI);
|
||||
setpoint = ((35/180.0f)*M_PI_F);
|
||||
|
||||
} else {
|
||||
setpoint = atanf((plane_data.wp_z - plane_data.alt) / calc_wp_distance());
|
||||
|
||||
if (setpoint < (-35.0f/180.0f)*F_M_PI)
|
||||
return (-35.0f/180.0f)*F_M_PI;
|
||||
if (setpoint < (-35.0f/180.0f)*M_PI_F)
|
||||
return (-35.0f/180.0f)*M_PI_F;
|
||||
|
||||
if (setpoint > (35/180.0f)*F_M_PI)
|
||||
return (-35.0f/180.0f)*F_M_PI;
|
||||
if (setpoint > (35/180.0f)*M_PI_F)
|
||||
return (-35.0f/180.0f)*M_PI_F;
|
||||
}
|
||||
|
||||
return setpoint;
|
||||
@@ -797,8 +805,8 @@ int fixedwing_control_main(int argc, char *argv[])
|
||||
// printf("Current Altitude: %i\n\n", (int)plane_data.alt);
|
||||
|
||||
printf("\nAttitude values: \n R:%i \n P: %i \n Y: %i \n\n RS: %i \n PS: %i \n YS: %i \n ",
|
||||
(int)(180.0f * plane_data.roll/F_M_PI), (int)(180.0f * plane_data.pitch/F_M_PI), (int)(180.0f * plane_data.yaw/F_M_PI),
|
||||
(int)(180.0f * plane_data.rollspeed/F_M_PI), (int)(180.0f * plane_data.pitchspeed/F_M_PI), (int)(180.0f * plane_data.yawspeed)/F_M_PI);
|
||||
(int)(180.0f * plane_data.roll/M_PI_F), (int)(180.0f * plane_data.pitch/M_PI_F), (int)(180.0f * plane_data.yaw/M_PI_F),
|
||||
(int)(180.0f * plane_data.rollspeed/M_PI_F), (int)(180.0f * plane_data.pitchspeed/M_PI_F), (int)(180.0f * plane_data.yawspeed)/M_PI_F);
|
||||
|
||||
// printf("\nBody Rates: \n P: %i \n Q: %i \n R: %i \n",
|
||||
// (int)(10000 * plane_data.p), (int)(10000 * plane_data.q), (int)(10000 * plane_data.r));
|
||||
|
||||
@@ -37,7 +37,7 @@
|
||||
|
||||
APPNAME = mavlink
|
||||
PRIORITY = SCHED_PRIORITY_DEFAULT
|
||||
STACKSIZE = 4096
|
||||
STACKSIZE = 2048
|
||||
|
||||
INCLUDES = $(TOPDIR)/../mavlink/include/mavlink
|
||||
|
||||
|
||||
@@ -1447,7 +1447,7 @@ int mavlink_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
thread_should_exit = false;
|
||||
mavlink_task = task_create("mavlink", SCHED_PRIORITY_DEFAULT, 4096, mavlink_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
mavlink_task = task_create("mavlink", SCHED_PRIORITY_DEFAULT, 4400, mavlink_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
thread_running = true;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
@@ -43,6 +43,7 @@
|
||||
#include <assert.h>
|
||||
#include <stdio.h>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <stdbool.h>
|
||||
#include <string.h>
|
||||
#include <systemlib/param/param.h>
|
||||
@@ -145,39 +146,54 @@ int mavlink_pm_send_param(param_t param)
|
||||
return mavlink_missionlib_send_message(&tx_msg);
|
||||
}
|
||||
|
||||
static const char *mavlink_parameter_file = "/eeprom/parameters";
|
||||
/**
|
||||
* @return 0 on success, -1 if device open failed, -2 if writing parameters failed
|
||||
*/
|
||||
static int
|
||||
mavlink_pm_save_eeprom()
|
||||
{
|
||||
const char* name = "/eeprom";
|
||||
int fd = open("/eeprom", O_WRONLY | O_CREAT | O_EXCL);
|
||||
unlink(mavlink_parameter_file);
|
||||
|
||||
if (fd < 0)
|
||||
warn(1, "opening '%s' failed", name);
|
||||
int fd = open(mavlink_parameter_file, O_WRONLY | O_CREAT | O_EXCL);
|
||||
|
||||
if (fd < 0) {
|
||||
warn("opening '%s' failed", mavlink_parameter_file);
|
||||
return -1;
|
||||
}
|
||||
|
||||
int result = param_export(fd, false);
|
||||
close(fd);
|
||||
|
||||
if (result < 0) {
|
||||
warn(1, "error exporting to '%s'", name);
|
||||
unlink(mavlink_parameter_file);
|
||||
warn("error exporting to '%s'", mavlink_parameter_file);
|
||||
return -2;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @return 0 on success, -1 if device open failed, -2 if writing parameters failed
|
||||
*/
|
||||
static int
|
||||
mavlink_pm_load_eeprom()
|
||||
{
|
||||
const char* name = "/eeprom";
|
||||
int fd = open(name, O_RDONLY);
|
||||
int fd = open(mavlink_parameter_file, O_RDONLY);
|
||||
|
||||
if (fd < 0)
|
||||
warn(1, "open '%s' failed", name);
|
||||
if (fd < 0) {
|
||||
warn("open '%s' failed", mavlink_parameter_file);
|
||||
return -1;
|
||||
}
|
||||
|
||||
int result = param_import(fd);
|
||||
close(fd);
|
||||
|
||||
if (result < 0)
|
||||
warn(1, "error importing from '%s'", name);
|
||||
if (result < 0) {
|
||||
warn("error importing from '%s'", mavlink_parameter_file);
|
||||
return -2;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -258,35 +274,42 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess
|
||||
|
||||
if (((int)(cmd_mavlink.param1)) == 0) {
|
||||
|
||||
if (OK == mavlink_pm_load_eeprom()) {
|
||||
/* read all parameters from EEPROM to RAM */
|
||||
int read_ret = mavlink_pm_load_eeprom();
|
||||
if (read_ret == OK) {
|
||||
//printf("[mavlink pm] Loaded EEPROM params in RAM\n");
|
||||
mavlink_missionlib_send_gcs_string("[mavlink pm] CMD Loaded EEPROM params in RAM");
|
||||
mavlink_missionlib_send_gcs_string("[mavlink pm] OK loaded EEPROM params");
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
|
||||
} else {
|
||||
//fprintf(stderr, "[mavlink pm] ERROR loading EEPROM params in RAM\n");
|
||||
mavlink_missionlib_send_gcs_string("[mavlink pm] ERROR loading EEPROM params in RAM");
|
||||
if (read_ret < -1) {
|
||||
mavlink_missionlib_send_gcs_string("[mavlink pm] ERR loading params from EEPROM");
|
||||
} else {
|
||||
mavlink_missionlib_send_gcs_string("[mavlink pm] ERR loading params, no EEPROM found");
|
||||
}
|
||||
result = MAV_RESULT_FAILED;
|
||||
}
|
||||
|
||||
/* Write all parameters from RAM to EEPROM */
|
||||
|
||||
} else if (((int)(cmd_mavlink.param1)) == 1) {
|
||||
|
||||
if (OK == mavlink_pm_save_eeprom()) {
|
||||
//printf("[mavlink pm] RAM params written to EEPROM\n");
|
||||
mavlink_missionlib_send_gcs_string("[mavlink pm] RAM params written to EEPROM");
|
||||
/* write all parameters from RAM to EEPROM */
|
||||
int write_ret = mavlink_pm_save_eeprom();
|
||||
if (write_ret == OK) {
|
||||
mavlink_missionlib_send_gcs_string("[mavlink pm] OK params written to EEPROM");
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
|
||||
} else {
|
||||
//fprintf(stderr, "[mavlink pm] ERROR writing RAM params to EEPROM\n");
|
||||
mavlink_missionlib_send_gcs_string("[mavlink pm] ERROR writing RAM params to EEPROM");
|
||||
if (write_ret < -1) {
|
||||
mavlink_missionlib_send_gcs_string("[mavlink pm] ERR writing params to EEPROM");
|
||||
} else {
|
||||
mavlink_missionlib_send_gcs_string("[mavlink pm] ERR writing params, no EEPROM found");
|
||||
}
|
||||
result = MAV_RESULT_FAILED;
|
||||
}
|
||||
|
||||
} else {
|
||||
//fprintf(stderr, "[mavlink pm] refusing unsupported storage request\n");
|
||||
mavlink_missionlib_send_gcs_string("[mavlink pm] refusing unsupported storage request");
|
||||
mavlink_missionlib_send_gcs_string("[mavlink pm] refusing unsupported STOR request");
|
||||
result = MAV_RESULT_UNSUPPORTED;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -51,8 +51,27 @@
|
||||
#include <float.h>
|
||||
#include <math.h>
|
||||
#include <systemlib/pid/pid.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <arch/board/up_hrt.h>
|
||||
|
||||
PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 0.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 0.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(MC_ATT_P, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(MC_ATT_D, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.0f);
|
||||
|
||||
void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
|
||||
const struct vehicle_attitude_s *att, const struct vehicle_status_s *status,
|
||||
struct actuator_controls_s *actuators, bool verbose)
|
||||
|
||||
@@ -96,16 +96,16 @@ int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *eul
|
||||
mag_values.y = raw->magnetometer_ga[1]*456.0f;
|
||||
mag_values.z = raw->magnetometer_ga[2]*456.0f;
|
||||
|
||||
static int i = 0;
|
||||
// static int i = 0;
|
||||
|
||||
if (i == 500) {
|
||||
printf("gyro: %8.4f\t%8.4f\t%8.4f\t accel: %8.4f\t%8.4f\t%8.4f\t mag: %8.4f\t%8.4f\t%8.4f\t\n",
|
||||
gyro_values.x, gyro_values.y, gyro_values.z,
|
||||
accel_values.x, accel_values.y, accel_values.z,
|
||||
mag_values.x, mag_values.y, mag_values.z);
|
||||
i = 0;
|
||||
}
|
||||
i++;
|
||||
// if (i == 500) {
|
||||
// printf("[att estim bm] gyro: %8.4f\t%8.4f\t%8.4f\t accel: %8.4f\t%8.4f\t%8.4f\t mag: %8.4f\t%8.4f\t%8.4f\t\n",
|
||||
// gyro_values.x, gyro_values.y, gyro_values.z,
|
||||
// accel_values.x, accel_values.y, accel_values.z,
|
||||
// mag_values.x, mag_values.y, mag_values.z);
|
||||
// i = 0;
|
||||
// }
|
||||
// i++;
|
||||
|
||||
attitude_blackmagic(&accel_values, &mag_values, &gyro_values);
|
||||
|
||||
|
||||
@@ -60,6 +60,8 @@
|
||||
#include <arch/board/up_adc.h>
|
||||
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/rc_channels.h>
|
||||
@@ -127,6 +129,66 @@ extern uint64_t ppm_last_valid_decode;
|
||||
/* ORB topic publishing our results */
|
||||
static orb_advert_t sensor_pub;
|
||||
|
||||
PARAM_DEFINE_FLOAT(SENSOR_GYRO_XOFF, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(SENSOR_GYRO_YOFF, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(SENSOR_GYRO_ZOFF, 0.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(SENSOR_MAG_XOFF, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(SENSOR_MAG_YOFF, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(SENSOR_MAG_ZOFF, 0.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(SENSOR_ACC_XOFF, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(SENSOR_ACC_YOFF, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(SENSOR_ACC_ZOFF, 0.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
|
||||
PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
|
||||
PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f);
|
||||
PARAM_DEFINE_FLOAT(RC1_REV, 1.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC2_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC2_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC2_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC2_REV, 1.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC3_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC3_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC3_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC3_REV, 1.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC4_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC4_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC4_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC4_REV, 1.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC5_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC5_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC5_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC5_REV, 1.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC6_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC6_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC6_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC6_REV, 1.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC7_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC7_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC7_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC7_REV, 1.0f);
|
||||
|
||||
PARAM_DEFINE_FLOAT(RC8_MIN, 1000);
|
||||
PARAM_DEFINE_FLOAT(RC8_TRIM, 1500);
|
||||
PARAM_DEFINE_FLOAT(RC8_MAX, 2000);
|
||||
PARAM_DEFINE_FLOAT(RC8_REV, 1.0f);
|
||||
|
||||
PARAM_DEFINE_INT32(RC_TYPE, 1); // 1 = FUTABA
|
||||
|
||||
PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
|
||||
PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
|
||||
PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
|
||||
PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
|
||||
PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5);
|
||||
|
||||
/**
|
||||
* Sensor readout and publishing.
|
||||
*
|
||||
|
||||
@@ -35,6 +35,10 @@
|
||||
* @file param.c
|
||||
*
|
||||
* Global parameter store.
|
||||
*
|
||||
* Note that it might make sense to convert this into a driver. That would
|
||||
* offer some interesting options regarding state for e.g. ORB advertisements
|
||||
* and background parameter saving.
|
||||
*/
|
||||
|
||||
#include <debug.h>
|
||||
@@ -46,10 +50,15 @@
|
||||
|
||||
#include <sys/stat.h>
|
||||
|
||||
#include <arch/board/up_hrt.h>
|
||||
|
||||
#include "systemlib/param/param.h"
|
||||
#include "systemlib/uthash/utarray.h"
|
||||
#include "systemlib/bson/tinybson.h"
|
||||
|
||||
#include "uORB/uORB.h"
|
||||
#include "uORB/topics/parameters.h"
|
||||
|
||||
#if 1
|
||||
# define debug(fmt, args...) do { warnx(fmt, ##args); } while(0)
|
||||
#else
|
||||
@@ -79,6 +88,9 @@ UT_array *param_values;
|
||||
/** array info for the modified parameters array */
|
||||
UT_icd param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL};
|
||||
|
||||
/** parameter update topic */
|
||||
ORB_DEFINE(parameter_update, struct parameter_update_s);
|
||||
|
||||
/** lock the parameter store */
|
||||
static void
|
||||
param_lock(void)
|
||||
@@ -369,6 +381,24 @@ param_set(param_t param, const void *val)
|
||||
|
||||
out:
|
||||
param_unlock();
|
||||
|
||||
/*
|
||||
* If we set something, now that we have unlocked, go ahead and advertise that
|
||||
* a thing has been set.
|
||||
*/
|
||||
if (result != 0) {
|
||||
struct parameter_update_s pup = { .timestamp = hrt_absolute_time() };
|
||||
|
||||
/*
|
||||
* Because we're a library, we can't keep a persistent advertisement
|
||||
* around, so if we succeed in updating the topic, we have to toss
|
||||
* the descriptor straight away.
|
||||
*/
|
||||
int param_topic = orb_advertise(ORB_ID(parameter_update), &pup);
|
||||
if (param_topic != -1)
|
||||
close(param_topic);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
@@ -407,6 +437,7 @@ param_export(int fd, bool only_unsaved)
|
||||
switch (param_type(s->param)) {
|
||||
case PARAM_TYPE_INT32:
|
||||
param_get(s->param, &i);
|
||||
|
||||
if (bson_encoder_append_int(&encoder, param_name(s->param), i)) {
|
||||
debug("BSON append failed for '%s'", param_name(s->param));
|
||||
goto out;
|
||||
@@ -416,6 +447,7 @@ param_export(int fd, bool only_unsaved)
|
||||
|
||||
case PARAM_TYPE_FLOAT:
|
||||
param_get(s->param, &f);
|
||||
|
||||
if (bson_encoder_append_double(&encoder, param_name(s->param), f)) {
|
||||
debug("BSON append failed for '%s'", param_name(s->param));
|
||||
goto out;
|
||||
@@ -424,7 +456,7 @@ param_export(int fd, bool only_unsaved)
|
||||
break;
|
||||
|
||||
case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
|
||||
if (bson_encoder_append_binary(&encoder,
|
||||
if (bson_encoder_append_binary(&encoder,
|
||||
param_name(s->param),
|
||||
BSON_BIN_BINARY,
|
||||
param_size(s->param),
|
||||
@@ -474,6 +506,7 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node)
|
||||
* ignore the node.
|
||||
*/
|
||||
param_t param = param_find(node->name);
|
||||
|
||||
if (param == PARAM_INVALID)
|
||||
return 0;
|
||||
|
||||
@@ -487,6 +520,7 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node)
|
||||
debug("unexpected type for '%s", node->name);
|
||||
goto out;
|
||||
}
|
||||
|
||||
i = node->i;
|
||||
v = &i;
|
||||
break;
|
||||
@@ -496,6 +530,7 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node)
|
||||
debug("unexpected type for '%s", node->name);
|
||||
goto out;
|
||||
}
|
||||
|
||||
f = node->d;
|
||||
v = &f;
|
||||
break;
|
||||
@@ -505,22 +540,28 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node)
|
||||
debug("unexpected subtype for '%s", node->name);
|
||||
goto out;
|
||||
}
|
||||
|
||||
if (bson_decoder_data_pending(decoder) != param_size(param)) {
|
||||
debug("bad size for '%s'", node->name);
|
||||
goto out;
|
||||
}
|
||||
|
||||
/* XXX check actual file data size? */
|
||||
tmp = malloc(param_size(param));
|
||||
|
||||
if (tmp == NULL) {
|
||||
debug("failed allocating for '%s'", node->name);
|
||||
goto out;
|
||||
}
|
||||
|
||||
if (bson_decoder_copy_data(decoder, tmp)) {
|
||||
debug("failed copying data for '%s'", node->name);
|
||||
goto out;
|
||||
}
|
||||
|
||||
v = tmp;
|
||||
break;
|
||||
|
||||
default:
|
||||
debug("unrecognised node type");
|
||||
goto out;
|
||||
@@ -530,6 +571,7 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node)
|
||||
debug("error setting value for '%s'", node->name);
|
||||
goto out;
|
||||
}
|
||||
|
||||
if (tmp != NULL) {
|
||||
free(tmp);
|
||||
tmp = NULL;
|
||||
@@ -538,8 +580,10 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node)
|
||||
result = 0;
|
||||
|
||||
out:
|
||||
|
||||
if (tmp != NULL)
|
||||
free(tmp);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
@@ -559,6 +603,7 @@ param_import(int fd)
|
||||
|
||||
while (!done) {
|
||||
result = bson_decoder_next(&decoder);
|
||||
|
||||
if (result != 0) {
|
||||
debug("error during BSON decode");
|
||||
break;
|
||||
|
||||
@@ -0,0 +1,59 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file parameters.h
|
||||
*
|
||||
* Actuator control topics - mixer inputs.
|
||||
*
|
||||
* Values published to these topics are the outputs of the vehicle control
|
||||
* system, and are expected to be mixed and used to drive the actuators
|
||||
* (servos, speed controls, etc.) that operate the vehicle.
|
||||
*
|
||||
* Each topic can be published by a single controller
|
||||
*/
|
||||
|
||||
#ifndef TOPIC_PARAMETERS_H
|
||||
#define TOPIC_PARAMETERS_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include "../uORB.h"
|
||||
|
||||
struct parameter_update_s {
|
||||
/** time at which the latest parameter was updated */
|
||||
uint64_t timestamp;
|
||||
};
|
||||
|
||||
ORB_DECLARE(parameter_update);
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user