diff options
author | tnaegeli <naegelit@student.ethz.ch> | 2012-10-09 16:31:04 +0200 |
---|---|---|
committer | tnaegeli <naegelit@student.ethz.ch> | 2012-10-09 16:31:04 +0200 |
commit | 613e12fcac07a17e6b9d25b05f58c8a1b9587f5e (patch) | |
tree | b6ce6ef89c1f4bbef7ada6f32a3342bb727d4d61 | |
parent | f292b03772ddf9a0ae72615248c65959a110d8e2 (diff) | |
download | px4-firmware-613e12fcac07a17e6b9d25b05f58c8a1b9587f5e.tar.gz px4-firmware-613e12fcac07a17e6b9d25b05f58c8a1b9587f5e.tar.bz2 px4-firmware-613e12fcac07a17e6b9d25b05f58c8a1b9587f5e.zip |
working offboard
-rwxr-xr-x | ROMFS/scripts/rcS | 1 | ||||
-rw-r--r-- | apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c | 6 | ||||
-rw-r--r-- | apps/commander/state_machine_helper.c | 6 | ||||
-rw-r--r-- | apps/mavlink/mavlink.c | 51 | ||||
-rw-r--r-- | apps/multirotor_att_control/multirotor_att_control_main.c | 62 | ||||
-rw-r--r-- | apps/multirotor_att_control/multirotor_rate_control.c | 9 | ||||
-rw-r--r-- | apps/sensors/sensor_params.c | 6 |
7 files changed, 79 insertions, 62 deletions
diff --git a/ROMFS/scripts/rcS b/ROMFS/scripts/rcS index 9e564e2cc..3377abe77 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 159a70701..66c58f74f 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 1e82bc417..57512bfd5 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 c13f462ea..0592d0377 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 40f1bbfde..7d5821d3f 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_manual_enabled) { + 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); + } + + /* 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 372b378d1..7532dffa2 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]); + + 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] ); + 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 e6f2f7e76..52316993e 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); |