|
|
|
|
@ -11,12 +11,8 @@
@@ -11,12 +11,8 @@
|
|
|
|
|
Mode::Mode(void) : |
|
|
|
|
g(blimp.g), |
|
|
|
|
g2(blimp.g2), |
|
|
|
|
// wp_nav(blimp.wp_nav),
|
|
|
|
|
// loiter_nav(blimp.loiter_nav),
|
|
|
|
|
// pos_control(blimp.pos_control),
|
|
|
|
|
inertial_nav(blimp.inertial_nav), |
|
|
|
|
ahrs(blimp.ahrs), |
|
|
|
|
// attitude_control(blimp.attitude_control),
|
|
|
|
|
motors(blimp.motors), |
|
|
|
|
channel_right(blimp.channel_right), |
|
|
|
|
channel_front(blimp.channel_front), |
|
|
|
|
@ -152,15 +148,6 @@ void Blimp::update_flight_mode()
@@ -152,15 +148,6 @@ void Blimp::update_flight_mode()
|
|
|
|
|
void Blimp::exit_mode(Mode *&old_flightmode, |
|
|
|
|
Mode *&new_flightmode) |
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
// smooth throttle transition when switching from manual to automatic flight modes
|
|
|
|
|
if (old_flightmode->has_manual_throttle() && !new_flightmode->has_manual_throttle() && motors->armed() && !ap.land_complete) { |
|
|
|
|
// this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle
|
|
|
|
|
// set_accel_throttle_I_from_pilot_throttle();
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// cancel any takeoffs in progress
|
|
|
|
|
// old_flightmode->takeoff_stop();
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// notify_flight_mode - sets notify object based on current flight mode. Only used for OreoLED notify device
|
|
|
|
|
@ -189,41 +176,8 @@ void Mode::get_pilot_desired_accelerations(float &right_out, float &front_out) c
@@ -189,41 +176,8 @@ void Mode::get_pilot_desired_accelerations(float &right_out, float &front_out) c
|
|
|
|
|
// fetch roll and pitch inputs
|
|
|
|
|
right_out = channel_right->get_control_in(); |
|
|
|
|
front_out = channel_front->get_control_in(); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// // do circular limit
|
|
|
|
|
// float total_in = norm(pitch_out, roll_out);
|
|
|
|
|
// if (total_in > angle_limit) {
|
|
|
|
|
// float ratio = angle_limit / total_in;
|
|
|
|
|
// roll_out *= ratio;
|
|
|
|
|
// pitch_out *= ratio;
|
|
|
|
|
// }
|
|
|
|
|
|
|
|
|
|
// do lateral tilt to euler roll conversion
|
|
|
|
|
// roll_out = (18000/M_PI) * atanf(cosf(pitch_out*(M_PI/18000))*tanf(roll_out*(M_PI/18000)));
|
|
|
|
|
|
|
|
|
|
// roll_out and pitch_out are returned
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// bool Mode::_TakeOff::triggered(const float target_climb_rate) const
|
|
|
|
|
// {
|
|
|
|
|
// if (!blimp.ap.land_complete) {
|
|
|
|
|
// // can't take off if we're already flying
|
|
|
|
|
// return false;
|
|
|
|
|
// }
|
|
|
|
|
// if (target_climb_rate <= 0.0f) {
|
|
|
|
|
// // can't takeoff unless we want to go up...
|
|
|
|
|
// return false;
|
|
|
|
|
// }
|
|
|
|
|
|
|
|
|
|
// if (blimp.motors->get_spool_state() != Fins::SpoolState::THROTTLE_UNLIMITED) {
|
|
|
|
|
// // hold aircraft on the ground until rotor speed runup has finished
|
|
|
|
|
// return false;
|
|
|
|
|
// }
|
|
|
|
|
|
|
|
|
|
// return true;
|
|
|
|
|
// }
|
|
|
|
|
|
|
|
|
|
bool Mode::is_disarmed_or_landed() const |
|
|
|
|
{ |
|
|
|
|
if (!motors->armed() || !blimp.ap.auto_armed || blimp.ap.land_complete) { |
|
|
|
|
@ -241,39 +195,6 @@ void Mode::zero_throttle_and_relax_ac(bool spool_up)
@@ -241,39 +195,6 @@ void Mode::zero_throttle_and_relax_ac(bool spool_up)
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// void Mode::zero_throttle_and_hold_attitude()
|
|
|
|
|
// {
|
|
|
|
|
// // run attitude controller
|
|
|
|
|
// attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f);
|
|
|
|
|
// attitude_control->set_throttle_out(0.0f, false, blimp.g.throttle_filt);
|
|
|
|
|
// }
|
|
|
|
|
|
|
|
|
|
// void Mode::make_safe_spool_down()
|
|
|
|
|
// {
|
|
|
|
|
// // command aircraft to initiate the shutdown process
|
|
|
|
|
// motors->set_desired_spool_state(Fins::DesiredSpoolState::GROUND_IDLE);
|
|
|
|
|
// switch (motors->get_spool_state()) {
|
|
|
|
|
|
|
|
|
|
// case Fins::SpoolState::SHUT_DOWN:
|
|
|
|
|
// case Fins::SpoolState::GROUND_IDLE:
|
|
|
|
|
// // relax controllers during idle states
|
|
|
|
|
// // attitude_control->reset_rate_controller_I_terms_smoothly();
|
|
|
|
|
// // attitude_control->set_yaw_target_to_current_heading();
|
|
|
|
|
// break;
|
|
|
|
|
|
|
|
|
|
// case Fins::SpoolState::SPOOLING_UP:
|
|
|
|
|
// case Fins::SpoolState::THROTTLE_UNLIMITED:
|
|
|
|
|
// case Fins::SpoolState::SPOOLING_DOWN:
|
|
|
|
|
// // while transitioning though active states continue to operate normally
|
|
|
|
|
// break;
|
|
|
|
|
// }
|
|
|
|
|
|
|
|
|
|
// // pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
|
|
|
|
|
// // pos_control->update_z_controller();
|
|
|
|
|
// // we may need to move this out
|
|
|
|
|
// // attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
|
|
|
|
|
// }
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|
get a height above ground estimate for landing |
|
|
|
|
*/ |
|
|
|
|
|
murata
4 years ago
committed by
Randy Mackay