From f3cb2cf8a3b3e002e665c82588e98e502ec3e285 Mon Sep 17 00:00:00 2001 From: tnaegeli Date: Wed, 3 Oct 2012 15:05:50 +0200 Subject: [PATCH 01/12] rate controller update --- .../multirotor_att_control_main.c | 19 +-- .../multirotor_rate_control.c | 120 +++++------------- 2 files changed, 43 insertions(+), 96 deletions(-) diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 9acdc6fd38..f3c0746e09 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -104,14 +104,14 @@ static void *rate_control_thread_main(void *arg) while (!thread_should_exit) { /* rate control at maximum rate */ /* wait for a sensor update, check for exit condition every 1000 ms */ - int ret = poll(&fds, 1, 1000); - - if (ret < 0) { - /* XXX this is seriously bad - should be an emergency */ - } else if (ret == 0) { - /* XXX this means no sensor data - should be critical or emergency */ - printf("[mc att control] WARNING: Not getting gyro data, no rate control\n"); - } else { +// int ret = poll(&fds, 1, 1000); +// +// if (ret < 0) { +// /* XXX this is seriously bad - should be an emergency */ +// } else if (ret == 0) { +// /* XXX this means no sensor data - should be critical or emergency */ +// printf("[mc att control] WARNING: Not getting gyro data, no rate control\n"); +// } else { /* get data */ orb_copy(ORB_ID(sensor_gyro), gyro_sub, &gyro_report); bool rates_sp_valid = false; @@ -133,7 +133,8 @@ static void *rate_control_thread_main(void *arg) multirotor_control_rates(&rates_sp, gyro_lp, &actuators); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); } - } +// } + usleep(2000); } return NULL; diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index b4eb3469b5..2a57f93f07 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -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)); @@ -151,106 +160,43 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static bool initialized = false; - /* initialize the pid controllers when the function is called for the first time */ - if (initialized == false) { - parameters_init(&h); - parameters_update(&h, &p); - - pid_init(&yaw_speed_controller, p.yawrate_p, 0, p.yawrate_i, p.yawrate_awu, - PID_MODE_DERIVATIV_SET); - pid_init(&pitch_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu, - PID_MODE_DERIVATIV_SET); - pid_init(&roll_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu, - PID_MODE_DERIVATIV_SET); - - initialized = true; - } /* load new parameters with lower rate */ - if (motor_skip_counter % 250 == 0) { + + if (motor_skip_counter % 500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); /* apply parameters */ + + + pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu); pid_set_parameters(&pitch_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu); pid_set_parameters(&roll_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu); } /* calculate current control outputs */ + float setpointXrate; + float setpointYrate; + float setpointZrate; + float setRollRate=rate_sp->roll; + float setPitchRate=rate_sp->pitch; + float setYawRate=rate_sp->yaw; + - /* control pitch (forward) output */ - float pitch_control = pid_calculate(&pitch_controller, rate_sp->pitch, - rates[1], 0.0f, deltaT); - /* control roll (left/right) output */ - float roll_control = pid_calculate(&roll_controller, rate_sp->roll, - rates[0], 0.0f, deltaT); - /* control yaw rate */ - float yaw_rate_control = pid_calculate(&yaw_speed_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); - - /* - * 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; - } + //x-axis + setpointXrate=p.attrate_p*(setRollRate-gyro_filtered[0]); + //Y-axis + setpointYrate=p.attrate_p*(setPitchRate-gyro_filtered[1]); + //Z-axis + setpointZrate=p.yawrate_p*(setYawRate-gyro_filtered[2]); - 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; - } - - actuators->control[0] = roll_control; - actuators->control[1] = pitch_control; - actuators->control[2] = yaw_rate_control; - actuators->control[3] = motor_thrust; + actuators->control[0] = setpointXrate; //roll + actuators->control[1] = setpointYrate; //pitch + actuators->control[2] = setpointZrate; //yaw + actuators->control[3] = rate_sp->thrust; motor_skip_counter++; } From 733975ed2d7b5906e35dbdebad52ee8fa9d92fd6 Mon Sep 17 00:00:00 2001 From: tnaegeli Date: Thu, 4 Oct 2012 09:28:04 +0200 Subject: [PATCH 02/12] fixed Rate controller --- apps/commander/commander.c | 15 ++-- .../multirotor_att_control_main.c | 75 ++++++++++++------- .../multirotor_rate_control.c | 27 ++----- nuttx/configs/px4fmu/nsh/defconfig | 4 +- 4 files changed, 63 insertions(+), 58 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 77e4da8501..124ac8aeb8 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1307,18 +1307,19 @@ int commander_thread_main(int argc, char *argv[]) //printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count); if (sp_man.override_mode_switch > STICK_ON_OFF_LIMIT) { - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = false; + current_status.flag_control_attitude_enabled = false; + current_status.flag_control_rates_enabled = true; update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); } else if (sp_man.override_mode_switch < -STICK_ON_OFF_LIMIT) { - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = false; - update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); + current_status.flag_control_attitude_enabled = false; + current_status.flag_control_rates_enabled = true; + update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); + //update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); } else { - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = false; + current_status.flag_control_attitude_enabled = false; + current_status.flag_control_rates_enabled = true; update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); } diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index ba7f08bc83..6b32a8ca39 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -94,24 +94,26 @@ static void *rate_control_thread_main(void *arg) int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - struct pollfd fds = { .fd = att_sub, .events = POLLIN }; + orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); + +// struct pollfd fds = { .fd = att_sub, .events = POLLIN }; struct vehicle_attitude_s vehicle_attitude; struct vehicle_rates_setpoint_s rates_sp; memset(&rates_sp, 0, sizeof(rates_sp)); float gyro_lp[3] = {0.0f, 0.0f, 0.0f}; - while (!thread_should_exit) { - /* rate control at maximum rate */ - /* wait for a sensor update, check for exit condition every 1000 ms */ - int ret = poll(&fds, 1, 1000); - - if (ret < 0) { - /* XXX this is seriously bad - should be an emergency */ - } else if (ret == 0) { - /* XXX this means no sensor data - should be critical or emergency */ - printf("[mc att control] WARNING: Not getting gyro data, no rate control\n"); - } else { + while (true) { +// /* rate control at maximum rate */ +// /* wait for a sensor update, check for exit condition every 1000 ms */ +// int ret = poll(&fds, 1, 1000); +// +// if (ret < 0) { +// /* XXX this is seriously bad - should be an emergency */ +// } else if (ret == 0) { +// /* XXX this means no sensor data - should be critical or emergency */ +// printf("[mc att control] WARNING: Not getting gyro data, no rate control\n"); +// } else { /* get current angular rate */ orb_copy(ORB_ID(vehicle_attitude), att_sub, &vehicle_attitude); /* get current rate setpoint */ @@ -124,18 +126,19 @@ static void *rate_control_thread_main(void *arg) /* perform local lowpass */ /* apply controller */ - if (state.flag_control_rates_enabled) { +// if (state.flag_control_rates_enabled) { /* lowpass gyros */ // XXX gyro_lp[0] = vehicle_attitude.rollspeed; gyro_lp[1] = vehicle_attitude.pitchspeed; gyro_lp[2] = vehicle_attitude.yawspeed; - multirotor_control_rates(&rates_sp, gyro_lp, &actuators); + //multirotor_control_rates(&rates_sp, gyro_lp, &actuators); + printf(".\n"); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - } - } - usleep(2000); +// } +// } + usleep(5000); } return NULL; @@ -184,7 +187,7 @@ mc_thread_main(int argc, char *argv[]) actuators.control[i] = 0.0f; } - actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); + orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); @@ -197,7 +200,7 @@ mc_thread_main(int argc, char *argv[]) /* ready, spawn pthread */ pthread_attr_t rate_control_attr; pthread_attr_init(&rate_control_attr); - pthread_attr_setstacksize(&rate_control_attr, 2048); + pthread_attr_setstacksize(&rate_control_attr, 4096); pthread_t rate_control_thread; pthread_create(&rate_control_thread, &rate_control_attr, rate_control_thread_main, NULL); @@ -277,15 +280,31 @@ mc_thread_main(int argc, char *argv[]) /** 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); - } +// /* 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); +// } + float gyro_lp[3] = {0.0f, 0.0f, 0.0f}; + + gyro_lp[0] = att.rollspeed; + gyro_lp[1] = att.pitchspeed; + gyro_lp[2] = att.yawspeed; + + rates_sp.roll = manual.roll; + rates_sp.pitch = manual.pitch; + rates_sp.yaw = manual.yaw; // XXX Hack, remove, switch to yaw rate controller + /* set yaw rate */ + rates_sp.yaw = manual.yaw; + rates_sp.thrust = manual.throttle; + rates_sp.timestamp = hrt_absolute_time(); + + multirotor_control_rates(&rates_sp, gyro_lp, &actuators); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); perf_end(mc_loop_perf); } @@ -356,7 +375,7 @@ int multirotor_att_control_main(int argc, char *argv[]) mc_task = task_spawn("multirotor_att_control", SCHED_RR, SCHED_PRIORITY_MAX - 15, - 4096, + 6000, mc_thread_main, NULL); exit(0); diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index 2a57f93f07..1d400f51bf 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -150,11 +150,6 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static int motor_skip_counter = 0; - // static PID_t yaw_pos_controller; - static PID_t yaw_speed_controller; - static PID_t pitch_controller; - static PID_t roll_controller; - static struct mc_rate_control_params p; static struct mc_rate_control_param_handles h; @@ -166,13 +161,6 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (motor_skip_counter % 500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - /* apply parameters */ - - - - pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu); - pid_set_parameters(&pitch_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu); - pid_set_parameters(&roll_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu); } /* calculate current control outputs */ @@ -183,15 +171,12 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, float setPitchRate=rate_sp->pitch; float setYawRate=rate_sp->yaw; - - //x-axis - setpointXrate=p.attrate_p*(setRollRate-gyro_filtered[0]); - //Y-axis - setpointYrate=p.attrate_p*(setPitchRate-gyro_filtered[1]); - //Z-axis - setpointZrate=p.yawrate_p*(setYawRate-gyro_filtered[2]); - - + //x-axis + setpointXrate=p.attrate_p*(setRollRate-rates[0]); + //Y-axis + setpointYrate=p.attrate_p*(setPitchRate-rates[1]); + //Z-axis + setpointZrate=p.yawrate_p*(setYawRate-rates[2]); actuators->control[0] = setpointXrate; //roll actuators->control[1] = setpointYrate; //pitch diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index e1ba862cf7..dd0c16af85 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -236,11 +236,11 @@ CONFIG_AT24XX_MTD_BLOCKSIZE=256 CONFIG_SERIAL_TERMIOS=y CONFIG_SERIAL_CONSOLE_REINIT=y -CONFIG_USART1_SERIAL_CONSOLE=y +CONFIG_USART1_SERIAL_CONSOLE=n CONFIG_USART2_SERIAL_CONSOLE=n CONFIG_USART3_SERIAL_CONSOLE=n CONFIG_UART4_SERIAL_CONSOLE=n -CONFIG_UART5_SERIAL_CONSOLE=n +CONFIG_UART5_SERIAL_CONSOLE=y CONFIG_USART6_SERIAL_CONSOLE=n #Mavlink messages can be bigger than 128 From 4c14e4f5f1c67ae0d139e4998058991a02fe4912 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 6 Oct 2012 16:08:07 -0700 Subject: [PATCH 03/12] Add a 'secret' subcommand to bl_update that manipulates the option bits to change the brown-out detector configuration. This is an experiment to see if we can improve the boot-time behavior when powered off noisy supplies. --- apps/systemcmds/bl_update/bl_update.c | 37 ++++++++++++++++++++++++++- 1 file changed, 36 insertions(+), 1 deletion(-) diff --git a/apps/systemcmds/bl_update/bl_update.c b/apps/systemcmds/bl_update/bl_update.c index 8bfc292348..ac3e93be14 100644 --- a/apps/systemcmds/bl_update/bl_update.c +++ b/apps/systemcmds/bl_update/bl_update.c @@ -56,11 +56,16 @@ __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 +177,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); + +} \ No newline at end of file From dd50c88f073926a550f0f1f5b08f931116dd4f8f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 8 Oct 2012 17:59:43 +0200 Subject: [PATCH 04/12] Fixed GPS lost issue, fixed accel scale initialization, fixed code style in rate controller --- apps/gps/ubx.c | 2 ++ apps/multirotor_att_control/multirotor_rate_control.c | 7 +++---- apps/sensors/sensor_params.c | 6 +++--- nuttx/configs/px4fmu/nsh/defconfig | 4 ++-- 4 files changed, 10 insertions(+), 9 deletions(-) diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c index 8a658b6230..b1655afd7a 100644 --- a/apps/gps/ubx.c +++ b/apps/gps/ubx.c @@ -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); diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index 372b378d19..5a5bffca92 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -166,18 +166,17 @@ 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\n", (double)p.yawrate_p); } /* calculate current control outputs */ /* control pitch (forward) output */ - float pitch_control = p.attrate_p * deltaT * (rate_sp->pitch-rates[1]); + float pitch_control = p.attrate_p * deltaT * (rate_sp->pitch - rates[1]); /* control roll (left/right) output */ - float roll_control = p.attrate_p * deltaT * (rate_sp->roll-rates[0] ); + float roll_control = p.attrate_p * deltaT * (rate_sp->roll - rates[0]); /* control yaw rate */ - float yaw_rate_control = p.yawrate_p * deltaT * (rate_sp->yaw-rates[2] ); + float yaw_rate_control = p.yawrate_p * deltaT * (rate_sp->yaw - rates[2]); actuators->control[0] = roll_control; actuators->control[1] = pitch_control; diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index e6f2f7e765..52316993e8 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -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); diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index 45934e800b..c2656217d4 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -236,11 +236,11 @@ CONFIG_AT24XX_MTD_BLOCKSIZE=256 CONFIG_SERIAL_TERMIOS=y CONFIG_SERIAL_CONSOLE_REINIT=y -CONFIG_USART1_SERIAL_CONSOLE=n +CONFIG_USART1_SERIAL_CONSOLE=y CONFIG_USART2_SERIAL_CONSOLE=n CONFIG_USART3_SERIAL_CONSOLE=n CONFIG_UART4_SERIAL_CONSOLE=n -CONFIG_UART5_SERIAL_CONSOLE=y +CONFIG_UART5_SERIAL_CONSOLE=n CONFIG_USART6_SERIAL_CONSOLE=n #Mavlink messages can be bigger than 128 From d068025fcda77d2d3b6e1f98c2cbf331229f8f6a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 8 Oct 2012 18:01:04 +0200 Subject: [PATCH 05/12] Fixed accel scale initialization --- apps/sensors/sensor_params.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index e6f2f7e765..52316993e8 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -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); From a2ab5e8691d55a1c98f76ca05c325aad85e2e542 Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 8 Oct 2012 22:37:18 -0700 Subject: [PATCH 06/12] Don't treat end-of-document-structure as an error. --- apps/systemlib/bson/tinybson.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/apps/systemlib/bson/tinybson.c b/apps/systemlib/bson/tinybson.c index 46ced013d4..10b736fd6b 100644 --- a/apps/systemlib/bson/tinybson.c +++ b/apps/systemlib/bson/tinybson.c @@ -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; From bd3f3b10317f58d20f635f4a3e6aee8fca8d9d2b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 9 Oct 2012 16:26:29 +0200 Subject: [PATCH 07/12] Sensor rate and throttle inversion fixes --- apps/drivers/bma180/bma180.cpp | 4 +- apps/drivers/l3gd20/l3gd20.cpp | 4 +- apps/sensors/sensors.cpp | 128 +++++++-------------------------- 3 files changed, 30 insertions(+), 106 deletions(-) diff --git a/apps/drivers/bma180/bma180.cpp b/apps/drivers/bma180/bma180.cpp index 8cd7f6a7c7..c554df9ae9 100644 --- a/apps/drivers/bma180/bma180.cpp +++ b/apps/drivers/bma180/bma180.cpp @@ -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: { diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/apps/drivers/l3gd20/l3gd20.cpp index bfdabe2734..9401fdd4a5 100644 --- a/apps/drivers/l3gd20/l3gd20.cpp +++ b/apps/drivers/l3gd20/l3gd20.cpp @@ -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: { diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 7106edc6bc..b84b58406f 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -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)) From 613e12fcac07a17e6b9d25b05f58c8a1b9587f5e Mon Sep 17 00:00:00 2001 From: tnaegeli Date: Tue, 9 Oct 2012 16:31:04 +0200 Subject: [PATCH 08/12] working offboard --- ROMFS/scripts/rcS | 1 + .../attitude_estimator_ekf_main.c | 6 +- apps/commander/state_machine_helper.c | 6 +- apps/mavlink/mavlink.c | 51 +++++++++------- .../multirotor_att_control_main.c | 60 ++++++++++--------- .../multirotor_rate_control.c | 11 +++- apps/sensors/sensor_params.c | 6 +- 7 files changed, 79 insertions(+), 62 deletions(-) diff --git a/ROMFS/scripts/rcS b/ROMFS/scripts/rcS index 9e564e2cc7..3377abe77a 100755 --- a/ROMFS/scripts/rcS +++ b/ROMFS/scripts/rcS @@ -33,6 +33,7 @@ fi usleep 500 + # # Look for an init script on the microSD card. # diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 159a70701e..66c58f74fd 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -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)); diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 1e82bc417b..57512bfd55 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -503,7 +503,7 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; current_status->flag_control_manual_enabled = true; //XXX /* enable attitude control per default */ - current_status->flag_control_attitude_enabled = false; + current_status->flag_control_attitude_enabled = true; current_status->flag_control_rates_enabled = true; if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); @@ -518,7 +518,7 @@ 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_attitude_enabled = false; + current_status->flag_control_attitude_enabled = true; current_status->flag_control_rates_enabled = true; if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); @@ -533,7 +533,7 @@ 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_attitude_enabled = false; + current_status->flag_control_attitude_enabled = true; current_status->flag_control_rates_enabled = true; if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index c13f462ea4..0592d03775 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -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) { diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 40f1bbfde5..7d5821d3ff 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -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,39 +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) { diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index 372b378d19..7532dffa26 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -144,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(); @@ -172,10 +174,13 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* calculate current control outputs */ /* control pitch (forward) output */ - float pitch_control = p.attrate_p * deltaT * (rate_sp->pitch-rates[1]); - /* control roll (left/right) output */ - float roll_control = p.attrate_p * deltaT * (rate_sp->roll-rates[0] ); + 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 * (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 * (rate_sp->yaw-rates[2] ); diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index e6f2f7e765..52316993e8 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -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); From 1e59a592a6a41a65e9e69814ae3d8cb62a061367 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 9 Oct 2012 22:45:36 -0700 Subject: [PATCH 09/12] Split the parameter load/save commands out of the 'eeprom' command, since that's not really the obvious place for them. Add parameter printing functionality (though, it's a mess due to %f being busted) Update the script examples to use the new command. --- ROMFS/scripts/rc.FMU_quad_x | 2 +- ROMFS/scripts/rc.PX4IOAR | 2 +- ROMFS/scripts/rc.standalone | 10 ++ apps/systemcmds/bl_update/bl_update.c | 1 - apps/systemcmds/eeprom/eeprom.c | 4 + apps/systemcmds/param/Makefile | 42 ++++++ apps/systemcmds/param/param.c | 185 ++++++++++++++++++++++++++ apps/systemlib/param/param.c | 58 ++++++-- apps/systemlib/param/param.h | 17 ++- nuttx/configs/px4fmu/nsh/appconfig | 1 + 10 files changed, 307 insertions(+), 15 deletions(-) create mode 100644 apps/systemcmds/param/Makefile create mode 100644 apps/systemcmds/param/param.c diff --git a/ROMFS/scripts/rc.FMU_quad_x b/ROMFS/scripts/rc.FMU_quad_x index 94ed2be18b..e9f07b4a29 100644 --- a/ROMFS/scripts/rc.FMU_quad_x +++ b/ROMFS/scripts/rc.FMU_quad_x @@ -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" diff --git a/ROMFS/scripts/rc.PX4IOAR b/ROMFS/scripts/rc.PX4IOAR index 532dd6a256..382d8e25c0 100644 --- a/ROMFS/scripts/rc.PX4IOAR +++ b/ROMFS/scripts/rc.PX4IOAR @@ -19,7 +19,7 @@ echo "[init] eeprom" eeprom start if [ -f /eeprom/parameters ] then - eeprom load_param /eeprom/parameters + param load fi # diff --git a/ROMFS/scripts/rc.standalone b/ROMFS/scripts/rc.standalone index 7dfd98a166..8ccdb577b5 100644 --- a/ROMFS/scripts/rc.standalone +++ b/ROMFS/scripts/rc.standalone @@ -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. # diff --git a/apps/systemcmds/bl_update/bl_update.c b/apps/systemcmds/bl_update/bl_update.c index ac3e93be14..752c01986a 100644 --- a/apps/systemcmds/bl_update/bl_update.c +++ b/apps/systemcmds/bl_update/bl_update.c @@ -51,7 +51,6 @@ #include #include "systemlib/systemlib.h" -#include "systemlib/param/param.h" #include "systemlib/err.h" __EXPORT int bl_update_main(int argc, char *argv[]); diff --git a/apps/systemcmds/eeprom/eeprom.c b/apps/systemcmds/eeprom/eeprom.c index a0b15f77b7..fa88fa09e5 100644 --- a/apps/systemcmds/eeprom/eeprom.c +++ b/apps/systemcmds/eeprom/eeprom.c @@ -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) diff --git a/apps/systemcmds/param/Makefile b/apps/systemcmds/param/Makefile new file mode 100644 index 0000000000..603746a206 --- /dev/null +++ b/apps/systemcmds/param/Makefile @@ -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 diff --git a/apps/systemcmds/param/param.c b/apps/systemcmds/param/param.c new file mode 100644 index 0000000000..199bffed5b --- /dev/null +++ b/apps/systemcmds/param/param.c @@ -0,0 +1,185 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * 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 + +#include +#include +#include +#include +#include +#include +#include + +#include + +#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 (warning, floating-point values are often printed with the decimal point wrong)\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("\n", 0 + param_type(param), param_size(param)); + return; + default: + printf("\n", 0 + param_type(param)); + return; + } + printf("\n", param); +} diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c index 0ab7c0ea30..9e886ea651 100644 --- a/apps/systemlib/param/param.c +++ b/apps/systemlib/param/param.c @@ -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 diff --git a/apps/systemlib/param/param.h b/apps/systemlib/param/param.h index ffce07a4ed..41e268db08 100644 --- a/apps/systemlib/param/param.h +++ b/apps/systemlib/param/param.h @@ -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. */ diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index e76c4cf482..be0a3d1d71 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -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 From 674b6552364ac1d66e9fd13c72c74c90f1a15d69 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 10 Oct 2012 23:43:57 -0700 Subject: [PATCH 10/12] Quick hack to print floating-point numbers that are powers of 10 less wrongly. --- nuttx/lib/stdio/lib_libdtoa.c | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/nuttx/lib/stdio/lib_libdtoa.c b/nuttx/lib/stdio/lib_libdtoa.c index 1e022a8ebe..667c49c535 100644 --- a/nuttx/lib/stdio/lib_libdtoa.c +++ b/nuttx/lib/stdio/lib_libdtoa.c @@ -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 */ From d62ec78ab835153ef3ba480a5a4110465ba34372 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 11 Oct 2012 00:23:13 -0700 Subject: [PATCH 11/12] Remove obsolete warning. --- apps/systemcmds/param/param.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/systemcmds/param/param.c b/apps/systemcmds/param/param.c index 199bffed5b..d70d15da4f 100644 --- a/apps/systemcmds/param/param.c +++ b/apps/systemcmds/param/param.c @@ -141,7 +141,7 @@ do_import(void) static void do_show(void) { - printf(" + = saved, * = unsaved (warning, floating-point values are often printed with the decimal point wrong)\n"); + printf(" + = saved, * = unsaved\n"); param_foreach(do_show_print, NULL, false); exit(0); From 4dbf7befe369ba00a73945a0193f0a061c271dc3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Oct 2012 12:25:30 +0200 Subject: [PATCH 12/12] Disable rate control, disable offset estimation --- .../attitude_estimator_ekf_main.c | 7 ++++--- apps/commander/state_machine_helper.c | 12 ++++++------ 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 66c58f74fd..1d4df87fe3 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -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)); diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 57512bfd55..7d766bcdb2 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -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 = true; + 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 = true; + 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 = true; + 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) {