Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 14:24:21 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

Merge branch 'master' of https://github.com/PX4/Firmware

Browse Source
This commit is contained in:
Doug Weibel
2012-10-13 16:38:18 -06:00
parent 77e6375920 4dbf7befe3
commit 0a0215338a
24 changed files with 498 additions and 265 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/scripts/rc.FMU_quad_x
+1 -1
View File
@@ -9,7 +9,7 @@ echo "[init] eeprom"
eeprom start
if [ -f /eeprom/parameters ]
then
eeprom load_param /eeprom/parameters
param load
fi
echo "[init] sensors"
ROMFS/scripts/rc.PX4IOAR
+1 -1
View File
@@ -19,7 +19,7 @@ echo "[init] eeprom"
eeprom start
if [ -f /eeprom/parameters ]
then
eeprom load_param /eeprom/parameters
param load
fi
#
ROMFS/scripts/rc.standalone
+10
View File
@@ -10,6 +10,16 @@ echo "[init] doing standalone PX4FMU startup..."
#
uorb start
#
# Init the EEPROM
#
echo "[init] eeprom"
eeprom start
if [ -f /eeprom/parameters ]
then
param load
fi
#
# Start the sensors.
#
ROMFS/scripts/rcS
+1
View File
@@ -33,6 +33,7 @@ fi
usleep 500
#
# Look for an init script on the microSD card.
#
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+8 -7
View File
@@ -257,7 +257,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
{ .fd = sub_raw, .events = POLLIN },
{ .fd = sub_params, .events = POLLIN }
};
int ret = poll(fds, 1, 1000);
int ret = poll(fds, 2, 1000);
if (ret < 0) {
/* XXX this is seriously bad - should be an emergency */
@@ -389,9 +389,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
// }
printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt));
printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f\n", (double)euler[0], (double)euler[1], (double)euler[2]);
printf("update rates gyro: %8.4f\taccel: %8.4f\tmag:%8.4f\n", (double)sensor_update_hz[0], (double)sensor_update_hz[1], (double)sensor_update_hz[2]);
//printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt));
//printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f\n", (double)euler[0], (double)euler[1], (double)euler[2]);
//printf("update rates gyro: %8.4f\taccel: %8.4f\tmag:%8.4f\n", (double)sensor_update_hz[0], (double)sensor_update_hz[1], (double)sensor_update_hz[2]);
// printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100),
// (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100),
// (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100));
@@ -418,9 +418,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
att.pitch = euler[1];
att.yaw = euler[2];
att.rollspeed = x_aposteriori[0];
att.pitchspeed = x_aposteriori[1];
att.yawspeed = x_aposteriori[2];
// XXX replace with x_apo after fix to filter
att.rollspeed = raw.gyro_rad_s[0]; //x_aposteriori[0];
att.pitchspeed = raw.gyro_rad_s[1]; //x_aposteriori[1];
att.yawspeed = raw.gyro_rad_s[2]; //x_aposteriori[2];
/* copy offsets */
memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
apps/commander/commander.c
+6 -4
View File
@@ -293,7 +293,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
struct sensor_combined_s raw;
/* 30 seconds */
const uint64_t calibration_interval_us = 45 * 1000000;
int calibration_interval_ms = 30 * 1000;
unsigned int calibration_counter = 0;
float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX};
@@ -312,10 +312,10 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
warn("WARNING: failed to set scale / offsets for mag");
close(fd);
mavlink_log_info(mavlink_fd, "[commander] Please rotate around all axes.");
mavlink_log_info(mavlink_fd, "[commander] Please rotate around X");
uint64_t calibration_start = hrt_absolute_time();
while ((hrt_absolute_time() - calibration_start) < calibration_interval_us) {
while ((hrt_absolute_time() - calibration_start)/1000 < calibration_interval_ms) {
/* wait blocking for new data */
struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
@@ -348,11 +348,13 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
calibration_counter++;
} else {
/* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "[commander] mag calibration aborted, please retry.");
mavlink_log_info(mavlink_fd, "[commander] mag cal canceled");
break;
}
}
mavlink_log_info(mavlink_fd, "[commander] mag calibration done");
/* disable calibration mode */
status->flag_preflight_mag_calibration = false;
state_machine_publish(status_pub, status, mavlink_fd);
apps/commander/state_machine_helper.c
+6 -6
View File
@@ -501,10 +501,10 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c
{
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
current_status->flag_control_manual_enabled = true; //XXX
current_status->flag_control_manual_enabled = true;
/* enable attitude control per default */
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = false;
current_status->flag_control_rates_enabled = false; // XXX
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
@@ -517,9 +517,9 @@ void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_
{
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED;
current_status->flag_control_manual_enabled = true; //XXX
current_status->flag_control_manual_enabled = true;
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = false;
current_status->flag_control_rates_enabled = false; // XXX
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
@@ -532,9 +532,9 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur
{
int old_mode = current_status->flight_mode;
current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
current_status->flag_control_manual_enabled = true; //XXX
current_status->flag_control_manual_enabled = true;
current_status->flag_control_attitude_enabled = true;
current_status->flag_control_rates_enabled = false;
current_status->flag_control_rates_enabled = false; // XXX
if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) {
apps/drivers/bma180/bma180.cpp
+2 -2
View File
@@ -410,8 +410,8 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT:
/* XXX 500Hz is just a wild guess */
return ioctl(filp, SENSORIOCSPOLLRATE, 500);
/* With internal low pass filters enabled, 250 Hz is sufficient */
return ioctl(filp, SENSORIOCSPOLLRATE, 250);
/* adjust to a legal polling interval in Hz */
default: {
apps/drivers/l3gd20/l3gd20.cpp
+2 -2
View File
@@ -420,8 +420,8 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT:
/* XXX 500Hz is just a wild guess */
return ioctl(filp, SENSORIOCSPOLLRATE, 500);
/* With internal low pass filters enabled, 250 Hz is sufficient */
return ioctl(filp, SENSORIOCSPOLLRATE, 250);
/* adjust to a legal polling interval in Hz */
default: {
apps/gps/ubx.c
+2
View File
@@ -781,6 +781,8 @@ void *ubx_watchdog_loop(void *args)
} else {
/* gps healthy */
ubx_success_count++;
ubx_healthy = true;
ubx_fail_count = 0;
if (!ubx_healthy && ubx_success_count == UBX_HEALTH_SUCCESS_COUNTER_LIMIT) {
//printf("[gps] ublox UBX module status ok (baud=%d)\r\n", current_gps_speed);
apps/mavlink/mavlink.c
+33 -22
View File
@@ -889,7 +889,7 @@ static void *uorb_receiveloop(void *arg)
/* copy rc channels into local buffer */
orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
/* Channels are sent in MAVLink main loop at a fixed interval */
mavlink_msg_rc_channels_raw_send(chan, rc.timestamp / 1000, 0, rc.chan[0].raw, rc.chan[1].raw, rc.chan[2].raw, rc.chan[3].raw,
mavlink_msg_rc_channels_raw_send(chan, rc.timestamp / 1000, 0, rc.chan[0].raw, rc.chan[1].raw, rc.chan[2].raw, rc.chan[3].raw,
rc.chan[4].raw, rc.chan[5].raw, rc.chan[6].raw, rc.chan[7].raw, rc.rssi);
}
@@ -1272,7 +1272,8 @@ void handleMessage(mavlink_message_t *msg)
if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) {
mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint;
mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint);
// printf("got MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT target_system=%u, sysid = %u\n", quad_motors_setpoint.target_system, mavlink_system.sysid);
//printf("got message\n");
//printf("got MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT target_system=%u, sysid = %u\n", mavlink_system.sysid, quad_motors_setpoint.mode);
if (mavlink_system.sysid < 4) {
/*
@@ -1282,19 +1283,28 @@ void handleMessage(mavlink_message_t *msg)
uint8_t ml_mode = 0;
bool ml_armed = false;
if (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED) {
ml_armed = true;
}
// if (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED) {
// ml_armed = true;
// }
switch (quad_motors_setpoint.mode) {
case 0:
ml_armed = false;
break;
case 1:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
break;
ml_armed = true;
break;
case 2:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
break;
ml_armed = true;
break;
case 3:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
break;
@@ -1306,7 +1316,14 @@ void handleMessage(mavlink_message_t *msg)
offboard_control_sp.p1 = quad_motors_setpoint.roll[mavlink_system.sysid] / (float)INT16_MAX;
offboard_control_sp.p2 = quad_motors_setpoint.pitch[mavlink_system.sysid] / (float)INT16_MAX;
offboard_control_sp.p3= quad_motors_setpoint.yaw[mavlink_system.sysid] / (float)INT16_MAX;
offboard_control_sp.p4 = quad_motors_setpoint.thrust[mavlink_system.sysid] / (float)UINT16_MAX;
offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid]/(float)UINT16_MAX;
//offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid] ;
if (quad_motors_setpoint.thrust[mavlink_system.sysid] ==0){
ml_armed = false;
}
offboard_control_sp.armed = ml_armed;
offboard_control_sp.mode = ml_mode;
@@ -1720,14 +1737,6 @@ int mavlink_thread_main(int argc, char *argv[])
/* all subscriptions are now active, set up initial guess about rate limits */
if (baudrate >= 460800) {
/* 200 Hz / 5 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 5);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 5);
/* 200 Hz / 5 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 5);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 3);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 5);
/* 5 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 200);
} else if (baudrate >= 230400) {
/* 200 Hz / 5 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20);
@@ -1741,13 +1750,13 @@ int mavlink_thread_main(int argc, char *argv[])
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100);
} else if (baudrate >= 115200) {
/* 50 Hz / 20 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 200);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 200);
/* 20 Hz / 50 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 20);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
/* 10 Hz / 100 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 100);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
/* 1 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 1000);
} else if (baudrate >= 57600) {
@@ -1824,7 +1833,9 @@ int mavlink_thread_main(int argc, char *argv[])
mavlink_pm_queued_send();
/* sleep quarter the time */
usleep(25000);
mavlink_pm_queued_send();
if (baudrate > 57600) {
mavlink_pm_queued_send();
}
/* sleep 10 ms */
usleep(10000);
apps/multirotor_att_control/multirotor_att_control_main.c
+33 -31
View File
@@ -165,8 +165,30 @@ mc_thread_main(int argc, char *argv[])
/** STEP 1: Define which input is the dominating control input */
if (state.flag_control_offboard_enabled) {
/* offboard inputs */
if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
rates_sp.roll = offboard_sp.p1;
rates_sp.pitch = offboard_sp.p2;
rates_sp.yaw = offboard_sp.p3;
rates_sp.thrust = offboard_sp.p4;
printf("thrust_rate=%8.4f\n",offboard_sp.p4);
rates_sp.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
att_sp.roll_body = offboard_sp.p1;
att_sp.pitch_body = offboard_sp.p2;
att_sp.yaw_body = offboard_sp.p3;
att_sp.thrust = offboard_sp.p4;
printf("thrust_att=%8.4f\n",offboard_sp.p4);
att_sp.timestamp = hrt_absolute_time();
/* STEP 2: publish the result to the vehicle actuators */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
if (state.flag_control_manual_enabled) {
/* decide wether we want rate or position input */
}
else if (state.flag_control_manual_enabled) {
/* manual inputs, from RC control or joystick */
if (state.flag_control_rates_enabled && !state.flag_control_attitude_enabled) {
@@ -188,7 +210,7 @@ mc_thread_main(int argc, char *argv[])
}
/* STEP 2: publish the result to the vehicle actuators */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
if (motor_test_mode) {
att_sp.roll_body = 0.0f;
att_sp.pitch_body = 0.0f;
@@ -199,38 +221,19 @@ mc_thread_main(int argc, char *argv[])
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
} else if (state.flag_control_offboard_enabled) {
/* offboard inputs */
if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
rates_sp.roll = offboard_sp.p1;
rates_sp.pitch = offboard_sp.p2;
rates_sp.yaw = offboard_sp.p3;
rates_sp.thrust = offboard_sp.p4;
rates_sp.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
att_sp.roll_body = offboard_sp.p1;
att_sp.pitch_body = offboard_sp.p2;
att_sp.yaw_body = offboard_sp.p3;
att_sp.thrust = offboard_sp.p4;
att_sp.timestamp = hrt_absolute_time();
/* STEP 2: publish the result to the vehicle actuators */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
/* decide wether we want rate or position input */
}
/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
/* run attitude controller */
if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) {
multirotor_control_attitude(&att_sp, &att, NULL, &actuators);
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
} else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) {
multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL);
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
}
if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) {
multirotor_control_attitude(&att_sp, &att, NULL, &actuators);
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
} else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) {
multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL);
orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
}
if (state.flag_control_rates_enabled) {
@@ -253,7 +256,6 @@ mc_thread_main(int argc, char *argv[])
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
}
perf_end(mc_loop_perf);
}
@@ -321,7 +323,7 @@ int multirotor_att_control_main(int argc, char *argv[])
mc_task = task_spawn("multirotor_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 15,
4096,
6000,
mc_thread_main,
NULL);
exit(0);
apps/multirotor_att_control/multirotor_rate_control.c
+21 -67
View File
@@ -56,18 +56,21 @@
// PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 0.1f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.2f); /* 0.15 F405 Flamewheel */
PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.0f);
//PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
//PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f);
PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 8.0f); /**< roughly < 500 deg/s limit */
struct mc_rate_control_params {
float yawrate_p;
float yawrate_d;
float yawrate_i;
float yawrate_awu;
float yawrate_lim;
float attrate_p;
float attrate_d;
float attrate_i;
float attrate_awu;
float attrate_lim;
@@ -79,11 +82,13 @@ struct mc_rate_control_param_handles {
param_t yawrate_p;
param_t yawrate_i;
param_t yawrate_d;
param_t yawrate_awu;
param_t yawrate_lim;
param_t attrate_p;
param_t attrate_i;
param_t attrate_d;
param_t attrate_awu;
param_t attrate_lim;
};
@@ -106,11 +111,13 @@ static int parameters_init(struct mc_rate_control_param_handles *h)
/* PID parameters */
h->yawrate_p = param_find("MC_YAWRATE_P");
h->yawrate_i = param_find("MC_YAWRATE_I");
h->yawrate_d = param_find("MC_YAWRATE_D");
h->yawrate_awu = param_find("MC_YAWRATE_AWU");
h->yawrate_lim = param_find("MC_YAWRATE_LIM");
h->attrate_p = param_find("MC_ATTRATE_P");
h->attrate_i = param_find("MC_ATTRATE_I");
h->attrate_d = param_find("MC_ATTRATE_D");
h->attrate_awu = param_find("MC_ATTRATE_AWU");
h->attrate_lim = param_find("MC_ATTRATE_LIM");
@@ -121,11 +128,13 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
{
param_get(h->yawrate_p, &(p->yawrate_p));
param_get(h->yawrate_i, &(p->yawrate_i));
param_get(h->yawrate_d, &(p->yawrate_d));
param_get(h->yawrate_awu, &(p->yawrate_awu));
param_get(h->yawrate_lim, &(p->yawrate_lim));
param_get(h->attrate_p, &(p->attrate_p));
param_get(h->attrate_i, &(p->attrate_i));
param_get(h->attrate_d, &(p->attrate_d));
param_get(h->attrate_awu, &(p->attrate_awu));
param_get(h->attrate_lim, &(p->attrate_lim));
@@ -135,6 +144,8 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
const float rates[], struct actuator_controls_s *actuators)
{
static float roll_control_last=0;
static float pitch_control_last=0;
static uint64_t last_run = 0;
const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time();
@@ -157,83 +168,26 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
if (motor_skip_counter % 2500 == 0) {
/* update parameters from storage */
parameters_update(&h, &p);
printf("p.yawrate_p: %8.4f", (double)p.yawrate_p);
printf("p.yawrate_p: %8.4f\n", (double)p.yawrate_p);
}
/* calculate current control outputs */
/* control pitch (forward) output */
float pitch_control = 5 * deltaT * (rates[1] - rate_sp->pitch);
float pitch_control = p.attrate_p * deltaT *(rate_sp->pitch-rates[1])-p.attrate_d*(pitch_control_last);
pitch_control_last=pitch_control;
/* control roll (left/right) output */
float roll_control = p.attrate_p * deltaT * (rates[0] - rate_sp->roll);
float roll_control = p.attrate_p * deltaT * (rate_sp->roll-rates[0])-p.attrate_d*(roll_control_last);
roll_control_last=roll_control;
/* control yaw rate */
float yaw_rate_control = p.yawrate_p * deltaT * (rates[2] - rate_sp->yaw);
/*
* compensate the vertical loss of thrust
* when thrust plane has an angle.
* start with a factor of 1.0 (no change)
*/
float zcompensation = 1.0f;
// if (fabsf(att->roll) > 0.3f) {
// zcompensation *= 1.04675160154f;
// } else {
// zcompensation *= 1.0f / cosf(att->roll);
// }
// if (fabsf(att->pitch) > 0.3f) {
// zcompensation *= 1.04675160154f;
// } else {
// zcompensation *= 1.0f / cosf(att->pitch);
// }
float motor_thrust = 0.0f;
motor_thrust = rate_sp->thrust;
/* compensate thrust vector for roll / pitch contributions */
motor_thrust *= zcompensation;
// /* limit yaw rate output */
// if (yaw_rate_control > p.yawrate_lim) {
// yaw_rate_control = p.yawrate_lim;
// yaw_speed_controller.saturated = 1;
// }
// if (yaw_rate_control < -p.yawrate_lim) {
// yaw_rate_control = -p.yawrate_lim;
// yaw_speed_controller.saturated = 1;
// }
// if (pitch_control > p.attrate_lim) {
// pitch_control = p.attrate_lim;
// pitch_controller.saturated = 1;
// }
// if (pitch_control < -p.attrate_lim) {
// pitch_control = -p.attrate_lim;
// pitch_controller.saturated = 1;
// }
// if (roll_control > p.attrate_lim) {
// roll_control = p.attrate_lim;
// roll_controller.saturated = 1;
// }
// if (roll_control < -p.attrate_lim) {
// roll_control = -p.attrate_lim;
// roll_controller.saturated = 1;
// }
float yaw_rate_control = p.yawrate_p * deltaT * (rate_sp->yaw-rates[2] );
actuators->control[0] = roll_control;
actuators->control[1] = pitch_control;
actuators->control[2] = yaw_rate_control;
actuators->control[3] = motor_thrust;
actuators->control[3] = rate_sp->thrust;
motor_skip_counter++;
}
apps/sensors/sensor_params.c
+3 -3
View File
@@ -56,9 +56,9 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_YOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_ZOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 0.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 0.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 0.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
apps/sensors/sensors.cpp
+26 -102
View File
@@ -101,8 +101,6 @@ extern "C" {
/* PPM Settings */
# define PPM_MIN 1000
# define PPM_MAX 2000
/* Internal resolution is 10000 */
# define PPM_SCALE 10000/((PPM_MAX-PPM_MIN)/2)
# define PPM_MID (PPM_MIN+PPM_MAX)/2
#endif
@@ -136,10 +134,6 @@ public:
private:
static const unsigned _rc_max_chan_count = 8; /**< maximum number of r/c channels we handle */
/* legacy sensor descriptors */
int _fd_bma180; /**< old accel driver */
int _fd_gyro_l3gd20; /**< old gyro driver */
#if CONFIG_HRT_PPM
hrt_abstime _ppm_last_valid; /**< last time we got a valid ppm signal */
@@ -334,8 +328,6 @@ Sensors *g_sensors;
}
Sensors::Sensors() :
_fd_bma180(-1),
_fd_gyro_l3gd20(-1),
_ppm_last_valid(0),
_fd_adc(-1),
@@ -562,19 +554,7 @@ Sensors::accel_init()
fd = open(ACCEL_DEVICE_PATH, 0);
if (fd < 0) {
warn("%s", ACCEL_DEVICE_PATH);
/* fall back to bma180 here (new driver would be better...) */
_fd_bma180 = open("/dev/bma180", O_RDONLY);
if (_fd_bma180 < 0) {
warn("/dev/bma180");
warn("FATAL: no accelerometer found");
}
/* discard first (junk) reading */
int16_t junk_buf[3];
read(_fd_bma180, junk_buf, sizeof(junk_buf));
warnx("using BMA180");
errx(1, "FATAL: no accelerometer found");
} else {
/* set the accel internal sampling rate up to at leat 500Hz */
ioctl(fd, ACCELIOCSSAMPLERATE, 500);
@@ -595,19 +575,7 @@ Sensors::gyro_init()
fd = open(GYRO_DEVICE_PATH, 0);
if (fd < 0) {
warn("%s", GYRO_DEVICE_PATH);
/* fall back to bma180 here (new driver would be better...) */
_fd_gyro_l3gd20 = open("/dev/l3gd20", O_RDONLY);
if (_fd_gyro_l3gd20 < 0) {
warn("/dev/l3gd20");
warn("FATAL: no gyro found");
}
/* discard first (junk) reading */
int16_t junk_buf[3];
read(_fd_gyro_l3gd20, junk_buf, sizeof(junk_buf));
warn("using L3GD20");
errx(1, "FATAL: no gyro found");
} else {
/* set the gyro internal sampling rate up to at leat 500Hz */
ioctl(fd, GYROIOCSSAMPLERATE, 500);
@@ -648,7 +616,7 @@ Sensors::baro_init()
fd = open(BARO_DEVICE_PATH, 0);
if (fd < 0) {
warn("%s", BARO_DEVICE_PATH);
errx(1, "FATAL: no barometer found");
warnx("No barometer found, ignoring");
}
/* set the driver to poll at 150Hz */
@@ -671,67 +639,36 @@ Sensors::adc_init()
void
Sensors::accel_poll(struct sensor_combined_s &raw)
{
struct accel_report accel_report;
bool accel_updated;
orb_check(_accel_sub, &accel_updated);
if (_fd_bma180 >= 0) {
/* do ORB emulation for BMA180 */
int16_t buf[3];
if (accel_updated) {
struct accel_report accel_report;
read(_fd_bma180, buf, sizeof(buf));
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
accel_report.timestamp = hrt_absolute_time();
raw.accelerometer_m_s2[0] = accel_report.x;
raw.accelerometer_m_s2[1] = accel_report.y;
raw.accelerometer_m_s2[2] = accel_report.z;
accel_report.x_raw = (buf[1] == -32768) ? 32767 : -buf[1];
accel_report.y_raw = buf[0];
accel_report.z_raw = buf[2];
raw.accelerometer_raw[0] = accel_report.x_raw;
raw.accelerometer_raw[1] = accel_report.y_raw;
raw.accelerometer_raw[2] = accel_report.z_raw;
const float range_g = 4.0f;
/* scale from 14 bit to m/s2 */
accel_report.x = (((accel_report.x_raw - _parameters.accel_offset[0]) * range_g) / 8192.0f) / 9.81f;
accel_report.y = (((accel_report.y_raw - _parameters.accel_offset[0]) * range_g) / 8192.0f) / 9.81f;
accel_report.z = (((accel_report.z_raw - _parameters.accel_offset[0]) * range_g) / 8192.0f) / 9.81f;
raw.accelerometer_counter++;
} else {
bool accel_updated;
orb_check(_accel_sub, &accel_updated);
if (accel_updated) {
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
raw.accelerometer_counter++;
}
}
raw.accelerometer_m_s2[0] = accel_report.x;
raw.accelerometer_m_s2[1] = accel_report.y;
raw.accelerometer_m_s2[2] = accel_report.z;
raw.accelerometer_raw[0] = accel_report.x_raw;
raw.accelerometer_raw[1] = accel_report.y_raw;
raw.accelerometer_raw[2] = accel_report.z_raw;
}
void
Sensors::gyro_poll(struct sensor_combined_s &raw)
{
struct gyro_report gyro_report;
bool gyro_updated;
orb_check(_gyro_sub, &gyro_updated);
if (_fd_gyro_l3gd20 >= 0) {
/* do ORB emulation for L3GD20 */
int16_t buf[3];
if (gyro_updated) {
struct gyro_report gyro_report;
read(_fd_gyro_l3gd20, buf, sizeof(buf));
gyro_report.timestamp = hrt_absolute_time();
gyro_report.x_raw = buf[1];
gyro_report.y_raw = ((buf[0] == -32768) ? 32767 : -buf[0]);
gyro_report.z_raw = buf[2];
/* scaling calculated as: raw * (1/(32768*(500/180*PI))) */
gyro_report.x = (gyro_report.x_raw - _parameters.gyro_offset[0]) * 0.000266316109f;
gyro_report.y = (gyro_report.y_raw - _parameters.gyro_offset[1]) * 0.000266316109f;
gyro_report.z = (gyro_report.z_raw - _parameters.gyro_offset[2]) * 0.000266316109f;
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
raw.gyro_rad_s[0] = gyro_report.x;
raw.gyro_rad_s[1] = gyro_report.y;
@@ -742,25 +679,6 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
raw.gyro_raw[2] = gyro_report.z_raw;
raw.gyro_counter++;
} else {
bool gyro_updated;
orb_check(_gyro_sub, &gyro_updated);
if (gyro_updated) {
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
raw.gyro_rad_s[0] = gyro_report.x;
raw.gyro_rad_s[1] = gyro_report.y;
raw.gyro_rad_s[2] = gyro_report.z;
raw.gyro_raw[0] = gyro_report.x_raw;
raw.gyro_raw[1] = gyro_report.y_raw;
raw.gyro_raw[2] = gyro_report.z_raw;
raw.gyro_counter++;
}
}
}
@@ -974,7 +892,13 @@ Sensors::ppm_poll()
}
/* reverse channel if required */
_rc.chan[i].scaled *= _parameters.rev[i];
if (i == _rc.function[THROTTLE]) {
if ((int)_parameters.rev[i] == -1) {
_rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled;
}
} else {
_rc.chan[i].scaled *= _parameters.rev[i];
}
/* handle any parameter-induced blowups */
if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled))
apps/systemcmds/bl_update/bl_update.c
+36 -2
View File
@@ -51,16 +51,20 @@
#include <arch/board/board.h>
#include "systemlib/systemlib.h"
#include "systemlib/param/param.h"
#include "systemlib/err.h"
__EXPORT int bl_update_main(int argc, char *argv[]);
static void setopt(void);
int
bl_update_main(int argc, char *argv[])
{
if (argc != 2)
errx(1, "missing firmware filename");
errx(1, "missing firmware filename or command");
if (!strcmp(argv[1], "setopt"))
setopt();
int fd = open(argv[1], O_RDONLY);
if (fd < 0)
@@ -172,3 +176,33 @@ flash_end:
free(buf);
exit(0);
}
static void
setopt(void)
{
volatile uint32_t *optcr = (volatile uint32_t *)0x40023c14;
const uint16_t opt_mask = (3 << 2); /* BOR_LEV bitmask */
const uint16_t opt_bits = (0 << 2); /* BOR = 0, setting for 2.7-3.6V operation */
if ((*optcr & opt_mask) == opt_bits)
errx(0, "option bits are already set as required");
/* unlock the control register */
volatile uint32_t *optkeyr = (volatile uint32_t *)0x40023c08;
*optkeyr = 0x08192a3bU;
*optkeyr = 0x4c5d6e7fU;
if (*optcr & 1)
errx(1, "option control register unlock failed");
/* program the new option value */
*optcr = (*optcr & ~opt_mask) | opt_bits | (1 << 1);
usleep(1000);
if ((*optcr & opt_mask) == opt_bits)
errx(0, "option bits set");
errx(1, "option bits setting failed; readback 0x%04x", *optcr);
}
apps/systemcmds/eeprom/eeprom.c
+4
View File
@@ -193,6 +193,8 @@ eeprom_save(const char *name)
if (!name)
err(1, "missing argument for device name, try '/eeprom/parameters'");
warnx("WARNING: 'eeprom save_param' deprecated - use 'param save' instead");
/* delete the file in case it exists */
unlink(name);
@@ -222,6 +224,8 @@ eeprom_load(const char *name)
if (!name)
err(1, "missing argument for device name, try '/eeprom/parameters'");
warnx("WARNING: 'eeprom load_param' deprecated - use 'param load' instead");
int fd = open(name, O_RDONLY);
if (fd < 0)
apps/systemcmds/param/Makefile
+42
View File
@@ -0,0 +1,42 @@
############################################################################
#
# 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.
#
############################################################################
#
# Build the parameters tool.
#
APPNAME = param
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 4096
include $(APPDIR)/mk/app.mk
apps/systemcmds/param/param.c
+185
View File
@@ -0,0 +1,185 @@
/****************************************************************************
*
* Copyright (C) 2012 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
* 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 param.c
*
* Parameter tool.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/stat.h>
#include <arch/board/board.h>
#include "systemlib/systemlib.h"
#include "systemlib/param/param.h"
#include "systemlib/err.h"
__EXPORT int param_main(int argc, char *argv[]);
static void do_save(void);
static void do_load(void);
static void do_import(void);
static void do_show(void);
static void do_show_print(void *arg, param_t param);
static const char *param_file_name = "/eeprom/parameters";
int
param_main(int argc, char *argv[])
{
if (argc >= 2) {
if (!strcmp(argv[1], "save"))
do_save();
if (!strcmp(argv[1], "load"))
do_load();
if (!strcmp(argv[1], "import"))
do_import();
if (!strcmp(argv[1], "show"))
do_show();
}
errx(1, "expected a command, try 'load', 'import', 'show' or 'save'\n");
}
static void
do_save(void)
{
/* delete the parameter file in case it exists */
unlink(param_file_name);
/* create the file */
int fd = open(param_file_name, O_WRONLY | O_CREAT | O_EXCL);
if (fd < 0)
err(1, "opening '%s' failed", param_file_name);
int result = param_export(fd, false);
close(fd);
if (result < 0) {
unlink(param_file_name);
errx(1, "error exporting to '%s'", param_file_name);
}
exit(0);
}
static void
do_load(void)
{
int fd = open(param_file_name, O_RDONLY);
if (fd < 0)
err(1, "open '%s'", param_file_name);
int result = param_load(fd);
close(fd);
if (result < 0)
errx(1, "error importing from '%s'", param_file_name);
exit(0);
}
static void
do_import(void)
{
int fd = open(param_file_name, O_RDONLY);
if (fd < 0)
err(1, "open '%s'", param_file_name);
int result = param_import(fd);
close(fd);
if (result < 0)
errx(1, "error importing from '%s'", param_file_name);
exit(0);
}
static void
do_show(void)
{
printf(" + = saved, * = unsaved\n");
param_foreach(do_show_print, NULL, false);
exit(0);
}
static void
do_show_print(void *arg, param_t param)
{
int32_t i;
float f;
printf("%c %s: ",
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
param_name(param));
/*
* This case can be expanded to handle printing common structure types.
*/
switch (param_type(param)) {
case PARAM_TYPE_INT32:
if (!param_get(param, &i)) {
printf("%d\n", i);
return;
}
break;
case PARAM_TYPE_FLOAT:
if (!param_get(param, &f)) {
printf("%4.4f\n", (double)f);
return;
}
break;
case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
printf("<struct type %d size %u>\n", 0 + param_type(param), param_size(param));
return;
default:
printf("<unknown type %d>\n", 0 + param_type(param));
return;
}
printf("<error fetching parameter %d>\n", param);
}
apps/systemlib/bson/tinybson.c
+3 -1
View File
@@ -106,7 +106,9 @@ bson_decoder_next(bson_decoder_t decoder)
/* if the nesting level is now zero, the top-level document is done */
if (decoder->nesting == 0) {
CODER_KILL(decoder, "nesting is zero, document is done");
/* like kill but not an error */
debug("nesting is zero, document is done");
decoder->fd = -1;
/* return end-of-file to the caller */
return 0;
apps/systemlib/param/param.c
+47 -11
View File
@@ -242,6 +242,25 @@ param_name(param_t param)
return NULL;
}
bool
param_value_is_default(param_t param)
{
return param_find_changed(param) ? false : true;
}
bool
param_value_unsaved(param_t param)
{
static struct param_wbuf_s *s;
s = param_find_changed(param);
if (s && s->unsaved)
return true;
return false;
}
enum param_type_e
param_type(param_t param)
{
@@ -330,8 +349,8 @@ param_get(param_t param, void *val)
return result;
}
int
param_set(param_t param, const void *val)
static int
param_set_internal(param_t param, const void *val, bool mark_saved)
{
int result = -1;
bool params_changed = false;
@@ -394,7 +413,7 @@ param_set(param_t param, const void *val)
goto out;
}
s->unsaved = true;
s->unsaved = !mark_saved;
params_changed = true;
result = 0;
}
@@ -412,6 +431,12 @@ out:
return result;
}
int
param_set(param_t param, const void *val)
{
return param_set_internal(param, val, false);
}
void
param_reset(param_t param)
{
@@ -535,6 +560,11 @@ out:
return result;
}
struct param_import_state
{
bool mark_saved;
};
static int
param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node)
{
@@ -542,13 +572,13 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node)
int32_t i;
void *v, *tmp = NULL;
int result = -1;
struct param_import_state *state = (struct param_import_state *)private;
/*
* EOO means the end of the parameter object. (Currently not supporting
* nested BSON objects).
*/
if (node->type == BSON_EOO) {
*(bool *)private = true;
debug("end of parameters");
return 0;
}
@@ -621,7 +651,7 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node)
goto out;
}
if (param_set(param, v)) {
if (param_set_internal(param, v, state->mark_saved)) {
debug("error setting value for '%s'", node->name);
goto out;
}
@@ -642,19 +672,19 @@ out:
return result;
}
int
param_import(int fd)
static int
param_import_internal(int fd, bool mark_saved)
{
bool done;
struct bson_decoder_s decoder;
int result = -1;
struct param_import_state state;
if (bson_decoder_init(&decoder, fd, param_import_callback, &done)) {
if (bson_decoder_init(&decoder, fd, param_import_callback, &state)) {
debug("decoder init failed");
goto out;
}
done = false;
state.mark_saved = mark_saved;
do {
result = bson_decoder_next(&decoder);
@@ -668,11 +698,17 @@ out:
return result;
}
int
param_import(int fd)
{
return param_import_internal(fd, false);
}
int
param_load(int fd)
{
param_reset_all();
return param_import(fd);
return param_import_internal(fd, true);
}
void
apps/systemlib/param/param.h
+16 -1
View File
@@ -121,6 +121,20 @@ __EXPORT int param_get_index(param_t param);
*/
__EXPORT const char *param_name(param_t param);
/**
* Test whether a parameter's value has changed from the default.
*
* @return If true, the parameter's value has not been changed from the default.
*/
__EXPORT bool param_value_is_default(param_t param);
/**
* Test whether a parameter's value has been changed but not saved.
*
* @return If true, the parameter's value has not been saved.
*/
__EXPORT bool param_value_unsaved(param_t param);
/**
* Obtain the type of a parameter.
*
@@ -160,7 +174,8 @@ __EXPORT int param_set(param_t param, const void *val);
/**
* Reset a parameter to its default value.
*
* This function frees any storage used by struct parameters, but scalar parameters
* This function frees any storage used by struct parameters, and returns the parameter
* to its default value.
*
* @param param A handle returned by param_find or passed by param_foreach.
*/
nuttx/configs/px4fmu/nsh/appconfig
+1
View File
@@ -53,6 +53,7 @@ CONFIGURED_APPS += systemcmds/boardinfo
CONFIGURED_APPS += systemcmds/mixer
CONFIGURED_APPS += systemcmds/eeprom
CONFIGURED_APPS += systemcmds/led
CONFIGURED_APPS += systemcmds/param
CONFIGURED_APPS += systemcmds/bl_update
#CONFIGURED_APPS += systemcmds/calibration
nuttx/lib/stdio/lib_libdtoa.c
+9 -2
View File
@@ -224,8 +224,15 @@ static void lib_dtoa(FAR struct lib_outstream_s *obj, int fmt, int prec,
for (i = expt; i > 0; i--)
{
obj->put(obj, *digits);
digits++;
if (*digits != '\0')
{
obj->put(obj, *digits);
digits++;
}
else
{
obj->put(obj, '0');
}
}
/* Get the length of the fractional part */