/**
* Initialize the module
* \return -1 if initialization failed
* \return 0 on success
*/
static int32_t uavoTaranisInitialize(void)
{
uint32_t sport_com = PIOS_COM_FRSKY_SPORT;
if (sport_com) {
frsky = PIOS_malloc(sizeof(struct frsky_sport_telemetry));
if (frsky != NULL) {
memset(frsky, 0x00, sizeof(struct frsky_sport_telemetry));
// These objects are registered on the TLM so it
// can intercept them from the telemetry stream
FlightBatteryStateInitialize();
FlightStatusInitialize();
PositionActualInitialize();
VelocityActualInitialize();
frsky->frsky_settings.use_current_sensor = false;
frsky->frsky_settings.batt_cell_count = 0;
frsky->frsky_settings.use_baro_sensor = false;
frsky->state = FRSKY_STATE_WAIT_POLL_REQUEST;
frsky->last_poll_time = PIOS_DELAY_GetuS();
frsky->ignore_rx_chars = 0;
frsky->scheduled_item = -1;
frsky->com = sport_com;
uint8_t i;
for (i = 0; i < NELEMENTS(frsky_value_items); i++)
frsky->item_last_triggered[i] = PIOS_DELAY_GetuS();
PIOS_COM_ChangeBaud(frsky->com, FRSKY_SPORT_BAUDRATE);
module_enabled = true;
return 0;
}
module_enabled = true;
return 0;
}
module_enabled = false;
return -1;
}
/**
* Send value item previously scheduled by frsky_schedule_next_itme()
* @returns true when item value was sended
*/
static bool frsky_send_scheduled_item(void)
{
int32_t item = frsky->scheduled_item;
if ((item >= 0) && (item < NELEMENTS(frsky_value_items))) {
frsky->item_last_triggered[item] = PIOS_DELAY_GetuS();
uint32_t value = 0;
if (frsky_value_items[item].encode_value(&frsky->frsky_settings, &value, false,
frsky_value_items[item].fn_arg)) {
frsky_send_frame(frsky->com, (uint16_t)(frsky_value_items[item].id), value, true);
return true;
}
}
return false;
}
int main() {
PIOS_SYS_Init();
PIOS_Board_Init();
PIOS_IAP_Init();
USB_connected = PIOS_USB_CheckAvailable(0);
if (PIOS_IAP_CheckRequest() == true) {
PIOS_DELAY_WaitmS(1000);
User_DFU_request = true;
PIOS_IAP_ClearRequest();
}
GO_dfu = (USB_connected == true) || (User_DFU_request == true);
if (GO_dfu == true) {
PIOS_Board_Init();
if (User_DFU_request == true)
DeviceState = DFUidle;
else
DeviceState = BLidle;
} else
JumpToApp = true;
uint32_t stopwatch = 0;
uint32_t prev_ticks = PIOS_DELAY_GetuS();
while (true) {
/* Update the stopwatch */
uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks);
prev_ticks += elapsed_ticks;
stopwatch += elapsed_ticks;
if (JumpToApp == true)
jump_to_app();
switch (DeviceState) {
case Last_operation_Success:
case uploadingStarting:
case DFUidle:
period1 = 5000;
sweep_steps1 = 100;
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
period2 = 0;
break;
case uploading:
period1 = 5000;
sweep_steps1 = 100;
period2 = 2500;
sweep_steps2 = 50;
break;
case downloading:
period1 = 2500;
sweep_steps1 = 50;
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
period2 = 0;
break;
case BLidle:
period1 = 0;
PIOS_LED_On(PIOS_LED_HEARTBEAT);
period2 = 0;
break;
default://error
period1 = 5000;
sweep_steps1 = 100;
period2 = 5000;
sweep_steps2 = 100;
}
if (period1 != 0) {
if (LedPWM(period1, sweep_steps1, stopwatch))
PIOS_LED_On(PIOS_LED_HEARTBEAT);
else
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
} else
PIOS_LED_On(PIOS_LED_HEARTBEAT);
if (period2 != 0) {
if (LedPWM(period2, sweep_steps2, stopwatch))
PIOS_LED_On(PIOS_LED_HEARTBEAT);
else
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
} else
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
if (stopwatch > 50 * 1000 * 1000)
stopwatch = 0;
if ((stopwatch > 6 * 1000 * 1000) && (DeviceState
== BLidle))
JumpToApp = true;
processRX();
DataDownload(start);
}
}
/**
* @brief Calculate time in microseconds since a previous time
* @param[in] t previous time
* @return time in us since previous time t.
*/
uint32_t PIOS_DELAY_GetuSSince(uint32_t t)
{
return (PIOS_DELAY_GetuS() + us_modulo - t) % us_modulo;
}
/**
* @brief Calculate time in microseconds since a previous time
* @param[in] t previous time
* @return time in us since previous time t.
*/
uint32_t PIOS_DELAY_GetuSSince(uint32_t t)
{
return (PIOS_DELAY_GetuS() - t);
}
int main()
{
PIOS_SYS_Init();
PIOS_Board_Init();
PIOS_IAP_Init();
// Make sure the brown out reset value for this chip
// is 2.7 volts
check_bor();
#ifdef PIOS_INCLUDE_USB
USB_connected = PIOS_USB_CheckAvailable(0);
#endif
if (PIOS_IAP_CheckRequest() == true) {
PIOS_DELAY_WaitmS(1000);
User_DFU_request = true;
PIOS_IAP_ClearRequest();
}
GO_dfu = (USB_connected == true) || (User_DFU_request == true);
if (GO_dfu == true) {
if (User_DFU_request == true) {
DeviceState = DFUidle;
} else {
DeviceState = BLidle;
}
} else {
JumpToApp = true;
}
uint32_t stopwatch = 0;
uint32_t prev_ticks = PIOS_DELAY_GetuS();
while (true) {
/* Update the stopwatch */
uint32_t elapsed_ticks = PIOS_DELAY_GetuSSince(prev_ticks);
prev_ticks += elapsed_ticks;
stopwatch += elapsed_ticks;
if (JumpToApp == true) {
jump_to_app();
}
switch (DeviceState) {
case Last_operation_Success:
case uploadingStarting:
case DFUidle:
period1 = 5000;
sweep_steps1 = 100;
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
period2 = 0;
break;
case uploading:
period1 = 5000;
sweep_steps1 = 100;
period2 = 2500;
sweep_steps2 = 50;
break;
case downloading:
period1 = 2500;
sweep_steps1 = 50;
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
period2 = 0;
break;
case BLidle:
period1 = 0;
PIOS_LED_On(PIOS_LED_HEARTBEAT);
period2 = 0;
break;
default: // error
period1 = 5000;
sweep_steps1 = 100;
period2 = 5000;
sweep_steps2 = 100;
}
if (period1 != 0) {
if (LedPWM(period1, sweep_steps1, stopwatch)) {
PIOS_LED_On(PIOS_LED_HEARTBEAT);
} else {
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
}
} else {
PIOS_LED_On(PIOS_LED_HEARTBEAT);
}
if (period2 != 0) {
if (LedPWM(period2, sweep_steps2, stopwatch)) {
PIOS_LED_On(PIOS_LED_HEARTBEAT);
} else {
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
}
} else {
PIOS_LED_Off(PIOS_LED_HEARTBEAT);
}
if (stopwatch > 50 * 1000 * 1000) {
stopwatch = 0;
}
if ((stopwatch > 6 * 1000 * 1000) && ((DeviceState == BLidle) || (DeviceState == DFUidle && !USB_connected))) {
JumpToApp = true;
}
processRX();
DataDownload(start);
}
}
void cruisecontrol_compute_factor(AttitudeStateData *attitude, float thrustDemand)
{
static float previous_angle;
static uint32_t previous_time = 0;
static bool previous_time_valid = false;
// For multiple, speedy flips this mainly strives to address the
// fact that (due to thrust delay) thrust didn't average straight
// down, but at an angle. For less speedy flips it acts like it
// used to. It can be turned off by setting power delay to 0.
// It takes significant time for the motors of a multi-copter to
// spin up. It takes significant time for the collective servo of
// a CP heli to move from one end to the other. Both of those are
// modeled here as linear, i.e. twice as much change takes twice
// as long. Given a correctly configured maximum delay time this
// code calculates how far in advance to start the control
// transition so that half way through the physical transition it
// is just crossing the transition angle.
// Example: Rotation rate = 360. Full stroke delay = 0.2
// Transition angle 90 degrees. Start the transition 0.1 second
// before 90 degrees (36 degrees at 360 deg/sec) and it will be
// complete 0.1 seconds after 90 degrees.
// Note that this code only handles the transition to/from inverted
// thrust. It doesn't handle the case where thrust is changed a
// lot in a small angle range when that range is close to 90 degrees.
// It doesn't handle the small constant "system delay" caused by the
// delay between reading sensors and actuators beginning to respond.
// It also assumes that the pilot is holding the throttle constant;
// when the pilot does change the throttle, the compensation is
// simply recalculated.
// This implementation of future thrust isn't perfect. That would
// probably require an iterative procedure for solving a
// transcendental equation of the form linear(x) = 1/cos(x). It's
// shortcomings generally don't hurt anything and work better than
// without it. It is designed to work perfectly if the pilot is
// using full thrust during flips and it is only activated if 70% or
// greater thrust is used.
uint32_t time = PIOS_DELAY_GetuS();
// Get roll and pitch angles, calculate combined angle, and begin
// the general algorithm.
// Example: 45 degrees roll plus 45 degrees pitch = 60 degrees
// Do it every 8th iteration to save CPU.
if (time != previous_time || previous_time_valid == false) {
float angle, angle_unmodified;
// spherical right triangle
// 0.0 <= angle <= 180.0
angle_unmodified = angle = RAD2DEG(acosf(cos_lookup_deg(attitude->Roll)
* cos_lookup_deg(attitude->Pitch)));
// Calculate rate as a combined (roll and pitch) bank angle
// change; in degrees per second. Rate is calculated over the
// most recent 8 loops through stabilization. We could have
// asked the gyros. This is probably cheaper.
if (previous_time_valid) {
float rate;
// rate can be negative.
rate = (angle - previous_angle) / ((float)(time - previous_time) / 1000000.0f);
// Define "within range" to be those transitions that should
// be executing now. Recall that each impulse transition is
// spread out over a range of time / angle.
// There is only one transition and the high power level for
// it is either:
// 1/fabsf(cos(angle)) * current thrust
// or max power factor * current thrust
// or full thrust
// You can cross the transition with angle either increasing
// or decreasing (rate positive or negative).
// Thrust is never boosted for negative values of
// thrustDemand (negative stick values)
//
// When the aircraft is upright, thrust is always boosted
// . for positive values of thrustDemand
// When the aircraft is inverted, thrust is sometimes
// . boosted or reversed (or combinations thereof) or zeroed
// . for positive values of thrustDemand
// It depends on the inverted power settings.
// Of course, you can set MaxPowerFactor to 1.0 to
// . effectively disable boost.
if (thrustDemand > 0.0f) {
// to enable the future thrust calculations, make sure
// there is a large enough transition that the result
// will be roughly on vs. off; without that, it can
// exaggerate the length of time the inverted to upright
// transition holds full throttle and reduce the length
// of time for full throttle when going upright to inverted.
if (thrustDemand > 0.7f) {
float thrust;
thrust = CruiseControlFactorToThrust(CruiseControlAngleToFactor((float)stabSettings.settings.CruiseControlMaxAngle), thrustDemand);
// determine if we are in range of the transition
// given the thrust at max_angle and thrustDemand
// (typically close to 1.0), change variable 'thrust' to
// be the proportion of the largest thrust change possible
// that occurs when going into inverted mode.
// Example: 'thrust' is 0.8 A quad has min_thrust set
// to 0.05 The difference is 0.75. The largest possible
// difference with this setup is 0.9 - 0.05 = 0.85, so
// the proportion is 0.75/0.85
// That is nearly a full throttle stroke.
// the 'thrust' variable is non-negative here
switch (stabSettings.settings.CruiseControlInvertedPowerOutput) {
case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_ZERO:
// normal multi-copter case, stroke is max to zero
// technically max to constant min_thrust
// can be used by CP
thrust = (thrust - CruiseControlLimitThrust(0.0f)) / stabSettings.cruiseControl.thrust_difference;
break;
case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_NORMAL:
// reversed but not boosted
// : CP heli case, stroke is max to -stick
// : thrust = (thrust - CruiseControlLimitThrust(-thrustDemand)) / stabSettings.cruiseControl.thrust_difference;
// else it is both unreversed and unboosted
// : simply turn off boost, stroke is max to +stick
// : thrust = (thrust - CruiseControlLimitThrust(thrustDemand)) / stabSettings.cruiseControl.thrust_difference;
thrust = (thrust - CruiseControlLimitThrust(
(stabSettings.settings.CruiseControlInvertedThrustReversing
== STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDTHRUSTREVERSING_REVERSED)
? -thrustDemand
: thrustDemand)) / stabSettings.cruiseControl.thrust_difference;
break;
case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_BOOSTED:
// if boosted and reversed
if (stabSettings.settings.CruiseControlInvertedThrustReversing
== STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDTHRUSTREVERSING_REVERSED) {
// CP heli case, stroke is max to min
thrust = (thrust - CruiseControlFactorToThrust(-CruiseControlAngleToFactor((float)stabSettings.settings.CruiseControlMaxAngle), thrustDemand)) / stabSettings.cruiseControl.thrust_difference;
}
// else it is boosted and unreversed so the throttle doesn't change
else {
// CP heli case, no transition, so stroke is zero
thrust = 0.0f;
}
break;
}
// 'thrust' is now the proportion of max stroke
// multiply this proportion of max stroke,
// times the max stroke time, to get this stroke time
// we only want half of this time before the transition
// (and half after the transition)
thrust *= stabSettings.cruiseControl.half_power_delay;
// 'thrust' is now the length of time for this stroke
// multiply that times angular rate to get the lead angle
thrust *= fabsf(rate);
// if the transition is within range we use it,
// else we just use the current calculated thrust
if ((float)stabSettings.settings.CruiseControlMaxAngle - thrust <= angle
&& angle <= (float)stabSettings.settings.CruiseControlMaxAngle + thrust) {
// default to a little above max angle
angle = (float)stabSettings.settings.CruiseControlMaxAngle + 0.01f;
// if roll direction is downward
// then thrust value is taken from below max angle
// by the code that knows about the transition angle
if (rate < 0.0f) {
angle -= 0.02f;
}
}
} // if thrust > 0.7; else just use the angle we already calculated
cruisecontrol_factor = CruiseControlAngleToFactor(angle);
} else { // if thrust > 0 set factor from angle; else
cruisecontrol_factor = 1.0f;
}
if (angle >= (float)stabSettings.settings.CruiseControlMaxAngle) {
switch (stabSettings.settings.CruiseControlInvertedPowerOutput) {
case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_ZERO:
cruisecontrol_factor = 0.0f;
break;
case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_NORMAL:
cruisecontrol_factor = 1.0f;
break;
case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_BOOSTED:
// no change, leave factor >= 1.0 alone
break;
}
if (stabSettings.settings.CruiseControlInvertedThrustReversing
== STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDTHRUSTREVERSING_REVERSED) {
cruisecontrol_factor = -cruisecontrol_factor;
}
}
} // if previous_time_valid i.e. we've got a rate; else leave (angle and) factor alone
previous_time = time;
previous_time_valid = true;
previous_angle = angle_unmodified;
} // every 8th time
}
