diff --git a/apps/ardrone_control/ardrone_control.c b/apps/ardrone_control/ardrone_control.c index f80fdf9710..f8c17fe2d0 100644 --- a/apps/ardrone_control/ardrone_control.c +++ b/apps/ardrone_control/ardrone_control.c @@ -58,7 +58,7 @@ #include #include #include -#include +#include #include #include @@ -177,14 +177,14 @@ int ardrone_control_main(int argc, char *argv[]) struct vehicle_status_s state; struct vehicle_attitude_s att; struct ardrone_control_s ar_control; - struct rc_channels_s rc; + struct manual_control_setpoint_s manual; struct sensor_combined_s raw; struct ardrone_motors_setpoint_s setpoint; /* subscribe to attitude, motor setpoints and system state */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); - int rc_sub = orb_subscribe(ORB_ID(rc_channels)); + int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); int setpoint_sub = orb_subscribe(ORB_ID(ardrone_motors_setpoint)); @@ -194,8 +194,8 @@ int ardrone_control_main(int argc, char *argv[]) while (1) { /* get a local copy of the vehicle state */ orb_copy(ORB_ID(vehicle_status), state_sub, &state); - /* get a local copy of rc */ - orb_copy(ORB_ID(rc_channels), rc_sub, &rc); + /* get a local copy of manual setpoint */ + orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); /* get a local copy of attitude */ orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); @@ -208,7 +208,7 @@ int ardrone_control_main(int argc, char *argv[]) position_control_thread_started = true; } - control_attitude(&rc, &att, &state, ardrone_pub, &ar_control); + control_attitude(0, 0, 0, 0, &att, &state, ardrone_pub, &ar_control); //No check for remote sticks to disarm in auto mode, land/disarm with ground station @@ -219,7 +219,8 @@ int ardrone_control_main(int argc, char *argv[]) control_rates(&raw, &setpoint); } else if (control_mode == CONTROL_MODE_ATTITUDE) { - control_attitude(&rc, &att, &state, ardrone_pub, &ar_control); + control_attitude(manual.roll, manual.pitch, manual.yaw, + manual.throttle, &att, &state, ardrone_pub, &ar_control); } } else { diff --git a/apps/ardrone_control/attitude_control.c b/apps/ardrone_control/attitude_control.c index f96d22fe68..d9942fc734 100644 --- a/apps/ardrone_control/attitude_control.c +++ b/apps/ardrone_control/attitude_control.c @@ -59,14 +59,19 @@ extern int gpios; #define CONTROL_PID_ATTITUDE_INTERVAL 5e-3 +void turn_xy_plane(const float_vect3 *vector, float yaw, + float_vect3 *result); +void navi2body_xy_plane(const float_vect3 *vector, const float yaw, + float_vect3 *result); + void turn_xy_plane(const float_vect3 *vector, float yaw, float_vect3 *result) { //turn clockwise static uint16_t counter; - result->x = (cos(yaw) * vector->x + sin(yaw) * vector->y); - result->y = (-sin(yaw) * vector->x + cos(yaw) * vector->y); + result->x = (cosf(yaw) * vector->x + sinf(yaw) * vector->y); + result->y = (-sinf(yaw) * vector->x + cosf(yaw) * vector->y); result->z = vector->z; //leave direction normal to xy-plane untouched counter++; @@ -84,7 +89,7 @@ void navi2body_xy_plane(const float_vect3 *vector, const float yaw, // result->z = vector->z; //leave direction normal to xy-plane untouched } -void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attitude_s *att, const struct vehicle_status_s *status, int ardrone_pub, struct ardrone_control_s *ar_control) +void control_attitude(float roll, float pitch, float yaw, float thrust, const struct vehicle_attitude_s *att, const struct vehicle_status_s *status, int ardrone_pub, struct ardrone_control_s *ar_control) { static int motor_skip_counter = 0; @@ -93,8 +98,13 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit static PID_t nick_controller; static PID_t roll_controller; - static const float min_gas = 10; - static const float max_gas = 512; + const float min_thrust = 0.02f; /**< 2% minimum thrust */ + const float max_thrust = 1.0f; /**< 100% max thrust */ + const float scaling = 512.0f; /**< 100% thrust equals a value of 512 */ + + const float min_gas = min_thrust * scaling; + const float max_gas = max_thrust * scaling; + static uint16_t motor_pwm[4] = {0, 0, 0, 0}; static float motor_calc[4] = {0.0f, 0.0f, 0.0f, 0.0f}; // static float remote_control_weight_z = 1; @@ -160,10 +170,10 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit /* load new parameters with lower rate */ if (motor_skip_counter % 50 == 0) { pid_set_parameters(&yaw_pos_controller, - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_P], - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_I], - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_D], - (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_AWU]); + global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_P], + global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_I], + global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_D], + global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_AWU]); pid_set_parameters(&yaw_speed_controller, (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_P], @@ -189,7 +199,7 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit } current_state = status->state_machine; - float_vect3 attitude_setpoint_bodyframe = {}; //this is the setpoint in the bodyframe "mixed" together from the setpoint from the remote and the setpoint from the position controller + float_vect3 attitude_setpoint_bodyframe = {.x = 0.0f, .y = 0.0f, .z = 0.0f}; //this is the setpoint in the bodyframe "mixed" together from the setpoint from the remote and the setpoint from the position controller if (current_state == SYSTEM_STATE_AUTO) { @@ -200,15 +210,15 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit float yaw_e = att->yaw - attitude_setpoint_navigationframe_from_positioncontroller.z; // don't turn around the wrong side (only works if yaw angle is between +- 180 degree) - if (yaw_e > M_PI) { + if (yaw_e > M_PI_F) { yaw_e -= 2.0f * M_PI_F; } - if (yaw_e < -M_PI) { + if (yaw_e < -M_PI_F) { yaw_e += 2.0f * M_PI_F; } - attitude_setpoint_navigationframe_from_positioncontroller.z = pid_calculate(&yaw_pos_controller, 0, yaw_e, 0, CONTROL_PID_ATTITUDE_INTERVAL); + attitude_setpoint_navigationframe_from_positioncontroller.z = pid_calculate(&yaw_pos_controller, 0.0f, yaw_e, 0.0f, CONTROL_PID_ATTITUDE_INTERVAL); /* limit control output */ @@ -234,9 +244,9 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit attitude_setpoint_bodyframe.z = attitude_setpoint_bodyframe_from_positioncontroller.z; } else if (current_state == SYSTEM_STATE_MANUAL) { - attitude_setpoint_bodyframe.x = -((float)rc->chan[rc->function[ROLL]].scale / 10000.0f) * M_PI_F / 8.0f; - attitude_setpoint_bodyframe.y = -((float)rc->chan[rc->function[PITCH]].scale / 10000.0f) * M_PI_F / 8.0f; - attitude_setpoint_bodyframe.z = -((float)rc->chan[rc->function[YAW]].scale / 10000.0f) * M_PI_F; + attitude_setpoint_bodyframe.x = -roll * M_PI_F / 8.0f; + attitude_setpoint_bodyframe.y = -pitch * M_PI_F / 8.0f; + attitude_setpoint_bodyframe.z = -yaw * M_PI_F; } /* add an attitude offset which needs to be estimated somewhere */ @@ -245,23 +255,23 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit /*Calculate Controllers*/ //control Nick - float nick = pid_calculate(&nick_controller, attitude_setpoint_bodyframe.y, att->pitch, att->pitchspeed, CONTROL_PID_ATTITUDE_INTERVAL); + float nick_control = pid_calculate(&nick_controller, attitude_setpoint_bodyframe.y, att->pitch, att->pitchspeed, CONTROL_PID_ATTITUDE_INTERVAL); //control Roll - float roll = pid_calculate(&roll_controller, attitude_setpoint_bodyframe.x, att->roll, att->rollspeed, CONTROL_PID_ATTITUDE_INTERVAL); + float roll_control = pid_calculate(&roll_controller, attitude_setpoint_bodyframe.x, att->roll, att->rollspeed, CONTROL_PID_ATTITUDE_INTERVAL); //control Yaw Speed - float yaw = pid_calculate(&yaw_speed_controller, attitude_setpoint_bodyframe.z, att->yawspeed, 0, CONTROL_PID_ATTITUDE_INTERVAL); //attitude_setpoint_bodyframe.z is yaw speed! + float yaw_rate_control = pid_calculate(&yaw_speed_controller, attitude_setpoint_bodyframe.z, att->yawspeed, 0.0f, CONTROL_PID_ATTITUDE_INTERVAL); //attitude_setpoint_bodyframe.z is yaw speed! //compensation to keep force in z-direction float zcompensation; - if (fabs(att->roll) > 0.5f) { + if (fabsf(att->roll) > 0.5f) { zcompensation = 1.13949393f; } else { zcompensation = 1.0f / cosf(att->roll); } - if (fabs(att->pitch) > 0.5f) { + if (fabsf(att->pitch) > 0.5f) { zcompensation *= 1.13949393f; } else { @@ -282,13 +292,13 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit // FLYING MODES if (current_state == SYSTEM_STATE_MANUAL) { - motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale / 20000.0f; + motor_thrust = thrust; } else if (current_state == SYSTEM_STATE_GROUND_READY || current_state == SYSTEM_STATE_STABILIZED || current_state == SYSTEM_STATE_AUTO || current_state == SYSTEM_STATE_MISSION_ABORT) { - motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale / 20000.0f; //TODO + motor_thrust = thrust; //TODO } else if (current_state == SYSTEM_STATE_EMCY_LANDING) { - motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale / 20000.0f; //TODO + motor_thrust = thrust; //TODO } else if (current_state == SYSTEM_STATE_EMCY_CUTOFF) { motor_thrust = 0; //immediately cut off thrust! @@ -301,39 +311,37 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit // Convertion to motor-step units motor_thrust *= zcompensation; - /* scale up from 0..1 to 10..512) */ - motor_thrust *= ((float)max_gas - min_gas); //limit control output //yawspeed - if (yaw > pid_yawspeed_lim) { - yaw = pid_yawspeed_lim; + if (yaw_rate_control > pid_yawspeed_lim) { + yaw_rate_control = pid_yawspeed_lim; yaw_speed_controller.saturated = 1; } - if (yaw < -pid_yawspeed_lim) { - yaw = -pid_yawspeed_lim; + if (yaw_rate_control < -pid_yawspeed_lim) { + yaw_rate_control = -pid_yawspeed_lim; yaw_speed_controller.saturated = 1; } - if (nick > pid_att_lim) { - nick = pid_att_lim; + if (nick_control > pid_att_lim) { + nick_control = pid_att_lim; nick_controller.saturated = 1; } - if (nick < -pid_att_lim) { - nick = -pid_att_lim; + if (nick_control < -pid_att_lim) { + nick_control = -pid_att_lim; nick_controller.saturated = 1; } - if (roll > pid_att_lim) { - roll = pid_att_lim; + if (roll_control > pid_att_lim) { + roll_control = pid_att_lim; roll_controller.saturated = 1; } - if (roll < -pid_att_lim) { - roll = -pid_att_lim; + if (roll_control < -pid_att_lim) { + roll_control = -pid_att_lim; roll_controller.saturated = 1; } @@ -342,44 +350,44 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit ar_control->setpoint_attitude[0] = attitude_setpoint_bodyframe.x; ar_control->setpoint_attitude[1] = attitude_setpoint_bodyframe.y; ar_control->setpoint_attitude[2] = attitude_setpoint_bodyframe.z; - ar_control->attitude_control_output[0] = roll; - ar_control->attitude_control_output[1] = nick; - ar_control->attitude_control_output[2] = yaw; + ar_control->attitude_control_output[0] = roll_control; + ar_control->attitude_control_output[1] = nick_control; + ar_control->attitude_control_output[2] = yaw_rate_control; ar_control->zcompensation = zcompensation; orb_publish(ORB_ID(ardrone_control), ardrone_pub, ar_control); - static float output_band = 0.f; - static float band_factor = 0.75f; - static float startpoint_full_control = 150.0f; //TODO - static float yaw_factor = 1.0f; + float output_band = 0.f; + float band_factor = 0.75f; + const float startpoint_full_control = 0.25f; /**< start full control at 25% thrust */ + float yaw_factor = 1.0f; - if (motor_thrust <= min_gas) { - motor_thrust = min_gas; + if (motor_thrust <= min_thrust) { + motor_thrust = min_thrust; output_band = 0.f; - } else if (motor_thrust < startpoint_full_control && motor_thrust > min_gas) { - output_band = band_factor * (motor_thrust - min_gas); + } else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) { + output_band = band_factor * (motor_thrust - min_thrust); - } else if (motor_thrust >= startpoint_full_control && motor_thrust < max_gas - band_factor * startpoint_full_control) { + } else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) { output_band = band_factor * startpoint_full_control; - } else if (motor_thrust >= max_gas - band_factor * startpoint_full_control) { - output_band = band_factor * (max_gas - motor_thrust); + } else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) { + output_band = band_factor * (max_thrust - motor_thrust); } //add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer // FRONT (MOTOR 1) - motor_calc[0] = motor_thrust + (roll / 2 + nick / 2 - yaw); + motor_calc[0] = motor_thrust + (roll_control / 2 + nick_control / 2 - yaw_rate_control); // RIGHT (MOTOR 2) - motor_calc[1] = motor_thrust + (-roll / 2 + nick / 2 + yaw); + motor_calc[1] = motor_thrust + (-roll_control / 2 + nick_control / 2 + yaw_rate_control); // BACK (MOTOR 3) - motor_calc[2] = motor_thrust + (-roll / 2 - nick / 2 - yaw); + motor_calc[2] = motor_thrust + (-roll_control / 2 - nick_control / 2 - yaw_rate_control); // LEFT (MOTOR 4) - motor_calc[3] = motor_thrust + (roll / 2 - nick / 2 + yaw); + motor_calc[3] = motor_thrust + (roll_control / 2 - nick_control / 2 + yaw_rate_control); // if we are not in the output band if (!(motor_calc[0] < motor_thrust + output_band && motor_calc[0] > motor_thrust - output_band @@ -389,16 +397,16 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit yaw_factor = 0.5f; // FRONT (MOTOR 1) - motor_calc[0] = motor_thrust + (roll / 2 + nick / 2 - yaw * yaw_factor); + motor_calc[0] = motor_thrust + (roll_control / 2 + nick_control / 2 - yaw_rate_control * yaw_factor); // RIGHT (MOTOR 2) - motor_calc[1] = motor_thrust + (-roll / 2 + nick / 2 + yaw * yaw_factor); + motor_calc[1] = motor_thrust + (-roll_control / 2 + nick_control / 2 + yaw_rate_control * yaw_factor); // BACK (MOTOR 3) - motor_calc[2] = motor_thrust + (-roll / 2 - nick / 2 - yaw * yaw_factor); + motor_calc[2] = motor_thrust + (-roll_control / 2 - nick_control / 2 - yaw_rate_control * yaw_factor); // LEFT (MOTOR 4) - motor_calc[3] = motor_thrust + (roll / 2 - nick / 2 + yaw * yaw_factor); + motor_calc[3] = motor_thrust + (roll_control / 2 - nick_control / 2 + yaw_rate_control * yaw_factor); } uint8_t i; @@ -414,14 +422,17 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit } } - // Write out actual thrust - motor_pwm[0] = (uint16_t) motor_calc[0]; - motor_pwm[1] = (uint16_t) motor_calc[1]; - motor_pwm[2] = (uint16_t) motor_calc[2]; - motor_pwm[3] = (uint16_t) motor_calc[3]; + /* set the motor values */ - // Keep motors spinning while armed + /* scale up from 0..1 to 10..512) */ + motor_pwm[0] = (uint16_t) motor_calc[0] * ((float)max_gas - min_gas) + min_gas; + motor_pwm[1] = (uint16_t) motor_calc[1] * ((float)max_gas - min_gas) + min_gas; + motor_pwm[2] = (uint16_t) motor_calc[2] * ((float)max_gas - min_gas) + min_gas; + motor_pwm[3] = (uint16_t) motor_calc[3] * ((float)max_gas - min_gas) + min_gas; + /* Keep motors spinning while armed and prevent overflows */ + + /* Failsafe logic - should never be necessary */ motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : 10; motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : 10; motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : 10; diff --git a/apps/ardrone_control/attitude_control.h b/apps/ardrone_control/attitude_control.h index 6f66926c0b..a93a511bae 100644 --- a/apps/ardrone_control/attitude_control.h +++ b/apps/ardrone_control/attitude_control.h @@ -45,7 +45,7 @@ #include #include -void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attitude_s *att, +void control_attitude(float roll, float pitch, float yaw, float thrust, const struct vehicle_attitude_s *att, const struct vehicle_status_s *status, int ardrone_pub, struct ardrone_control_s *ar_control); diff --git a/apps/commander/commander.c b/apps/commander/commander.c index dc56472233..3663503d15 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -631,8 +631,10 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } - -static void *command_handling_loop(void *arg) //handles commands which come from the mavlink app +/** + * Handle commands sent by the ground control station via MAVLink. + */ +static void *command_handling_loop(void *arg) { /* Set thread name */ prctl(PR_SET_NAME, "commander cmd handler", getpid()); @@ -645,7 +647,7 @@ static void *command_handling_loop(void *arg) //handles commands which come fro struct pollfd fds[1] = { { .fd = cmd_sub, .events = POLLIN } }; if (poll(fds, 1, 5000) == 0) { - /* timeout, but this is no problem */ + /* timeout, but this is no problem, silently ignore */ } else { /* got command */ orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); @@ -785,7 +787,8 @@ int commander_main(int argc, char *argv[]) stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); if (stat_pub < 0) { - printf("[commander] ERROR: orb_advertise failed.\n"); + printf("[commander] ERROR: orb_advertise for topic vehicle_status failed.\n"); + exit(ERROR); } /* make sure we are in preflight state */ @@ -1035,6 +1038,8 @@ int commander_main(int argc, char *argv[]) int16_t rc_yaw_scale = rc.chan[rc.function[YAW]].scale; int16_t rc_throttle_scale = rc.chan[rc.function[THROTTLE]].scale; int16_t mode_switch_rc_value = rc.chan[rc.function[OVERRIDE]].scale; + /* Check the value of the rc channel of the mode switch */ + mode_switch_rc_value = rc.chan[rc.function[OVERRIDE]].scale; /* check if left stick is in lower left position --> switch to standby state */ if (rc_yaw_scale < -STICK_ON_OFF_LIMIT && rc_throttle_scale < STICK_THRUST_RANGE*0.2f) { //TODO: remove hardcoded values @@ -1061,9 +1066,6 @@ int commander_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); - /* Check the value of the rc channel of the mode switch */ - mode_switch_rc_value = rc.chan[rc.function[OVERRIDE]].scale; - if (mode_switch_rc_value > STICK_ON_OFF_LIMIT) { update_state_machine_mode_manual(stat_pub, ¤t_status); @@ -1074,6 +1076,9 @@ int commander_main(int argc, char *argv[]) update_state_machine_mode_stabilized(stat_pub, ¤t_status); } + /* Publish RC signal */ + + /* handle the case where RC signal was regained */ if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[commander] RECOVERY - RC SIGNAL GAINED!"); current_status.rc_signal_lost = false; diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c index b456c4dfb8..098db4456b 100644 --- a/apps/sensors/sensors.c +++ b/apps/sensors/sensors.c @@ -63,6 +63,7 @@ #include #include #include +#include #include #include "sensors.h" @@ -383,10 +384,23 @@ int sensors_main(int argc, char *argv[]) pthread_mutex_init(&sensors_read_ready_mutex, NULL); pthread_cond_init(&sensors_read_ready, NULL); - /* advertise the topic and make the initial publication */ + /* advertise the sensor_combined topic and make the initial publication */ sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); publishing = true; + /* advertise the manual_control topic */ + struct manual_control_setpoint_s manual_control = { .mode = ROLLPOS_PITCHPOS_YAWRATE_THROTTLE, + .roll = 0.0f, + .pitch = 0.0f, + .yaw = 0.0f, + .throttle = 0.0f }; + + int manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); + + if (manual_control_pub < 0) { + printf("[sensors] ERROR: orb_advertise for topic manual_control_setpoint failed.\n"); + } + /* advertise the rc topic */ struct rc_channels_s rc; memset(&rc, 0, sizeof(rc)); @@ -451,23 +465,23 @@ int sensors_main(int argc, char *argv[]) /* Update RC scalings and function mappings */ - rc.chan[0].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC1_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC1_MIN]) / 2) + rc.chan[0].scaling_factor = (1 / ((global_data_parameter_storage->pm.param_values[PARAM_RC1_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC1_MIN]) / 2) * global_data_parameter_storage->pm.param_values[PARAM_RC1_REV]); rc.chan[0].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC1_TRIM]; - rc.chan[1].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC2_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC2_MIN]) / 2) + rc.chan[1].scaling_factor = (1 / ((global_data_parameter_storage->pm.param_values[PARAM_RC2_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC2_MIN]) / 2) * global_data_parameter_storage->pm.param_values[PARAM_RC2_REV]); rc.chan[1].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC2_TRIM]; - rc.chan[2].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC3_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC3_MIN]) / 2) + rc.chan[2].scaling_factor = (1 / ((global_data_parameter_storage->pm.param_values[PARAM_RC3_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC3_MIN]) / 2) * global_data_parameter_storage->pm.param_values[PARAM_RC3_REV]); rc.chan[2].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC3_TRIM]; - rc.chan[3].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC4_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC4_MIN]) / 2) + rc.chan[3].scaling_factor = (1 / ((global_data_parameter_storage->pm.param_values[PARAM_RC4_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC4_MIN]) / 2) * global_data_parameter_storage->pm.param_values[PARAM_RC4_REV]); rc.chan[3].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC4_TRIM]; - rc.chan[4].scaling_factor = (10000 / ((global_data_parameter_storage->pm.param_values[PARAM_RC5_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC5_MIN]) / 2) + rc.chan[4].scaling_factor = (1 / ((global_data_parameter_storage->pm.param_values[PARAM_RC5_MAX] - global_data_parameter_storage->pm.param_values[PARAM_RC5_MIN]) / 2) * global_data_parameter_storage->pm.param_values[PARAM_RC5_REV]); rc.chan[4].mid = (uint16_t)global_data_parameter_storage->pm.param_values[PARAM_RC5_TRIM]; @@ -708,20 +722,48 @@ int sensors_main(int argc, char *argv[]) /* require at least two channels * to consider the signal valid + * check that decoded measurement is up to date */ if (ppm_decoded_channels > 1 && (hrt_absolute_time() - ppm_last_valid_decode) < 45000) { /* Read out values from HRT */ for (int i = 0; i < ppm_decoded_channels; i++) { rc.chan[i].raw = ppm_buffer[i]; /* Set the range to +-, then scale up */ - rc.chan[i].scale = (ppm_buffer[i] - rc.chan[i].mid) * rc.chan[i].scaling_factor; + rc.chan[i].scale = (ppm_buffer[i] - rc.chan[i].mid) * rc.chan[i].scaling_factor * 10000; + rc.chan[i].scaled = (ppm_buffer[i] - rc.chan[i].mid) * rc.chan[i].scaling_factor; } rc.chan_count = ppm_decoded_channels; - rc.timestamp = ppm_last_valid_decode; + /* publish a few lines of code later if set to true */ ppm_updated = true; + + /* roll input */ + manual_control.roll = rc.chan[rc.function[ROLL]].scaled; + if (manual_control.roll < -1.0f) manual_control.roll = -1.0f; + if (manual_control.roll > 1.0f) manual_control.roll = 1.0f; + + /* pitch input */ + manual_control.pitch = rc.chan[rc.function[PITCH]].scaled; + if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f; + if (manual_control.pitch > 1.0f) manual_control.pitch = 1.0f; + + /* yaw input */ + manual_control.yaw = rc.chan[rc.function[YAW]].scaled; + if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f; + if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f; + + /* throttle input */ + manual_control.throttle = rc.chan[rc.function[THROTTLE]].scaled; + if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; + if (manual_control.throttle > 2.0f) manual_control.throttle = 2.0f; + + /* mode switch input */ + manual_control.override_mode_switch = rc.chan[rc.function[OVERRIDE]].scaled; + if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f; + if (manual_control.override_mode_switch > 1.0f) manual_control.override_mode_switch = 1.0f; + } ppmcounter = 0; } @@ -832,6 +874,7 @@ int sensors_main(int argc, char *argv[]) if (ppm_updated) { orb_publish(ORB_ID(rc_channels), rc_pub, &rc); + orb_publish(ORB_ID(manual_control_setpoint), manual_control_pub, &manual_control); } #endif diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index 98e6c29873..2b12939a38 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -100,7 +100,10 @@ ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoi ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setpoint_s); #include "topics/vehicle_attitude_setpoint.h" -ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_s); +ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s); + +#include "topics/manual_control_setpoint.h" +ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s); #include "topics/actuator_controls.h" ORB_DEFINE(actuator_controls_0, struct actuator_controls_s); diff --git a/apps/uORB/topics/manual_control_setpoint.h b/apps/uORB/topics/manual_control_setpoint.h new file mode 100644 index 0000000000..b01c7438d9 --- /dev/null +++ b/apps/uORB/topics/manual_control_setpoint.h @@ -0,0 +1,95 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: @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 manual_control_setpoint.h + * Definition of the manual_control_setpoint uORB topic. + */ + +#ifndef TOPIC_MANUAL_CONTROL_SETPOINT_H_ +#define TOPIC_MANUAL_CONTROL_SETPOINT_H_ + +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** + * Defines how RC channels map to control inputs. + * + * The default mode on quadrotors and fixed wing is + * roll and pitch position of the right stick and + * throttle and yaw rate on the left stick + */ +enum MANUAL_CONTROL_MODE +{ + DIRECT = 0, + ROLLPOS_PITCHPOS_YAWRATE_THROTTLE = 1, + ROLLRATE_PITCHRATE_YAWRATE_THROTTLE = 2, + ROLLPOS_PITCHPOS_YAWPOS_THROTTLE = 3 +}; + +struct manual_control_setpoint_s { + + enum MANUAL_CONTROL_MODE mode; /**< The current control inputs mode */ + float roll; /**< roll / roll rate input */ + float pitch; + float yaw; + float throttle; + + float override_mode_switch; + + float ailerons; + float elevator; + float rudder; + float flaps; + + float aux1_cam_pan; + float aux2_cam_tilt; + float aux3_cam_zoom; + float aux4_cam_roll; + +}; /**< manual control inputs */ + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(manual_control_setpoint); + +#endif diff --git a/apps/uORB/topics/vehicle_attitude_setpoint.h b/apps/uORB/topics/vehicle_attitude_setpoint.h index 6b37574028..6a19080075 100644 --- a/apps/uORB/topics/vehicle_attitude_setpoint.h +++ b/apps/uORB/topics/vehicle_attitude_setpoint.h @@ -61,8 +61,14 @@ struct vehicle_attitude_setpoint_s float yaw_tait_bryan; /**< Tait-Bryan angle in NED frame */ float tait_bryan_valid; /**< Set to true if Tait-Bryan angles are valid */ + float roll_body; /**< body angle in NED frame */ + float pitch_body; /**< body angle in NED frame */ + float yaw_body; /**< body angle in NED frame */ + float body_valid; /**< Set to true if Tait-Bryan angles are valid */ + float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */ bool R_valid; /**< Set to true if rotation matrix is valid */ + float thrust; /**< Thrust in Newton the power system should generate */ };