mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-05 21:52:10 +01:00
POI orbit + some code run thru code formatter
This commit is contained in:
sambas
parent
6989468d17
commit
589290dbd7
@ -32,8 +32,8 @@
|
||||
// no direct UAVObject usage allowed in this file
|
||||
|
||||
// private functions
|
||||
static void path_endpoint( float * start_point, float * end_point, float * cur_point, struct path_status * status);
|
||||
static void path_vector( float * start_point, float * end_point, float * cur_point, struct path_status * status);
|
||||
static void path_endpoint(float * start_point, float * end_point, float * cur_point, struct path_status * status);
|
||||
static void path_vector(float * start_point, float * end_point, float * cur_point, struct path_status * status);
|
||||
static void path_circle(float * start_point, float * end_point, float * cur_point, struct path_status * status, bool clockwise);
|
||||
|
||||
/**
|
||||
@ -46,7 +46,7 @@ static void path_circle(float * start_point, float * end_point, float * cur_poin
|
||||
*/
|
||||
void path_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status, uint8_t mode)
|
||||
{
|
||||
switch(mode) {
|
||||
switch (mode) {
|
||||
case PATHDESIRED_MODE_FLYVECTOR:
|
||||
case PATHDESIRED_MODE_DRIVEVECTOR:
|
||||
return path_vector(start_point, end_point, cur_point, status);
|
||||
@ -75,7 +75,7 @@ void path_progress(float * start_point, float * end_point, float * cur_point, st
|
||||
* @param[in] cur_point Current location
|
||||
* @param[out] status Structure containing progress along path and deviation
|
||||
*/
|
||||
static void path_endpoint( float * start_point, float * end_point, float * cur_point, struct path_status * status)
|
||||
static void path_endpoint(float * start_point, float * end_point, float * cur_point, struct path_status * status)
|
||||
{
|
||||
float path_north, path_east, diff_north, diff_east;
|
||||
float dist_path, dist_diff;
|
||||
@ -91,10 +91,10 @@ static void path_endpoint( float * start_point, float * end_point, float * cur_p
|
||||
diff_north = end_point[0] - cur_point[0];
|
||||
diff_east = end_point[1] - cur_point[1];
|
||||
|
||||
dist_diff = sqrtf( diff_north * diff_north + diff_east * diff_east );
|
||||
dist_path = sqrtf( path_north * path_north + path_east * path_east );
|
||||
dist_diff = sqrtf(diff_north * diff_north + diff_east * diff_east);
|
||||
dist_path = sqrtf(path_north * path_north + path_east * path_east);
|
||||
|
||||
if(dist_diff < 1e-6 ) {
|
||||
if (dist_diff < 1e-6) {
|
||||
status->fractional_progress = 1;
|
||||
status->error = 0;
|
||||
status->path_direction[0] = status->path_direction[1] = 0;
|
||||
@ -117,7 +117,7 @@ static void path_endpoint( float * start_point, float * end_point, float * cur_p
|
||||
* @param[in] cur_point Current location
|
||||
* @param[out] status Structure containing progress along path and deviation
|
||||
*/
|
||||
static void path_vector( float * start_point, float * end_point, float * cur_point, struct path_status * status)
|
||||
static void path_vector(float * start_point, float * end_point, float * cur_point, struct path_status * status)
|
||||
{
|
||||
float path_north, path_east, diff_north, diff_east;
|
||||
float dist_path;
|
||||
@ -133,13 +133,13 @@ static void path_vector( float * start_point, float * end_point, float * cur_poi
|
||||
diff_east = cur_point[1] - start_point[1];
|
||||
|
||||
dot = path_north * diff_north + path_east * diff_east;
|
||||
dist_path = sqrtf( path_north * path_north + path_east * path_east );
|
||||
dist_path = sqrtf(path_north * path_north + path_east * path_east);
|
||||
|
||||
if(dist_path < 1e-6) {
|
||||
if (dist_path < 1e-6) {
|
||||
// if the path is too short, we cannot determine vector direction.
|
||||
// Fly towards the endpoint to prevent flying away,
|
||||
// but assume progress=1 either way.
|
||||
path_endpoint( start_point, end_point, cur_point, status );
|
||||
path_endpoint(start_point, end_point, cur_point, status);
|
||||
status->fractional_progress = 1;
|
||||
return;
|
||||
}
|
||||
@ -174,7 +174,7 @@ static void path_vector( float * start_point, float * end_point, float * cur_poi
|
||||
static void path_circle(float * start_point, float * end_point, float * cur_point, struct path_status * status, bool clockwise)
|
||||
{
|
||||
float radius_north, radius_east, diff_north, diff_east;
|
||||
float radius,cradius;
|
||||
float radius, cradius;
|
||||
float normal[2];
|
||||
float progress;
|
||||
float a_diff, a_radius;
|
||||
@ -187,8 +187,8 @@ static void path_circle(float * start_point, float * end_point, float * cur_poin
|
||||
diff_north = cur_point[0] - end_point[0];
|
||||
diff_east = cur_point[1] - end_point[1];
|
||||
|
||||
radius = sqrtf( powf(radius_north,2) + powf(radius_east,2) );
|
||||
cradius = sqrtf( powf(diff_north,2) + powf(diff_east,2) );
|
||||
radius = sqrtf(powf(radius_north, 2) + powf(radius_east, 2));
|
||||
cradius = sqrtf(powf(diff_north, 2) + powf(diff_east, 2));
|
||||
|
||||
if (cradius < 1e-6) {
|
||||
// cradius is zero, just fly somewhere and make sure correction is still a normal
|
||||
@ -211,35 +211,37 @@ static void path_circle(float * start_point, float * end_point, float * cur_poin
|
||||
normal[1] = -diff_north / cradius;
|
||||
}
|
||||
|
||||
|
||||
// normalize progress to 0..1
|
||||
a_diff = atan2f( diff_north, diff_east);
|
||||
a_radius = atan2f( radius_north, radius_east);
|
||||
a_diff = atan2f(diff_north, diff_east);
|
||||
a_radius = atan2f(radius_north, radius_east);
|
||||
|
||||
if(a_diff<0)
|
||||
a_diff+=2*M_PI;
|
||||
if(a_radius<0)
|
||||
a_radius+=2*M_PI;
|
||||
if (a_diff < 0) {
|
||||
a_diff += 2 * M_PI;
|
||||
}
|
||||
if (a_radius < 0) {
|
||||
a_radius += 2 * M_PI;
|
||||
}
|
||||
|
||||
progress = (a_diff - a_radius + M_PI) / (2 * M_PI);
|
||||
|
||||
if(progress<0)
|
||||
progress+=1.0;
|
||||
else if(progress>=1)
|
||||
progress-=1.0;
|
||||
|
||||
if(clockwise)
|
||||
{
|
||||
progress=1-progress;
|
||||
if (progress < 0) {
|
||||
progress += 1.0;
|
||||
} else if (progress >= 1) {
|
||||
progress -= 1.0;
|
||||
}
|
||||
|
||||
if (clockwise) {
|
||||
progress = 1 - progress;
|
||||
}
|
||||
|
||||
status->fractional_progress = progress;
|
||||
|
||||
// error is current radius minus wanted radius - positive if too close
|
||||
status->error = radius - cradius;
|
||||
|
||||
// Compute direction to correct error
|
||||
status->correction_direction[0] = (status->error>0?1:-1) * diff_north / cradius;
|
||||
status->correction_direction[1] = (status->error>0?1:-1) * diff_east / cradius;
|
||||
status->correction_direction[0] = (status->error > 0 ? 1 : -1) * diff_north / cradius;
|
||||
status->correction_direction[1] = (status->error > 0 ? 1 : -1) * diff_east / cradius;
|
||||
|
||||
// Compute direction to travel
|
||||
status->path_direction[0] = normal[0];
|
||||
|
||||
@ -131,8 +131,8 @@ static void altitudeTask(void *parameters)
|
||||
|
||||
#if defined(PIOS_INCLUDE_HCSR04)
|
||||
SonarAltitudeData sonardata;
|
||||
int32_t value=0,timeout=5;
|
||||
float coeff=0.25,height_out=0,height_in=0;
|
||||
int32_t value = 0, timeout = 5;
|
||||
float coeff = 0.25, height_out = 0, height_in = 0;
|
||||
if(hwsettings_rcvrport==HWSETTINGS_CC_RCVRPORT_DISABLED) {
|
||||
PIOS_HCSR04_Trigger();
|
||||
}
|
||||
@ -149,25 +149,23 @@ static void altitudeTask(void *parameters)
|
||||
#if defined(PIOS_INCLUDE_HCSR04)
|
||||
// Compute the current altitude
|
||||
if(hwsettings_rcvrport==HWSETTINGS_CC_RCVRPORT_DISABLED) {
|
||||
if(PIOS_HCSR04_Completed())
|
||||
{
|
||||
if(PIOS_HCSR04_Completed()) {
|
||||
value = PIOS_HCSR04_Get();
|
||||
if((value>100) && (value < 15000)) //from 3.4cm to 5.1m
|
||||
{
|
||||
height_in = value*0.00034f/2.0f;
|
||||
//from 3.4cm to 5.1m
|
||||
if((value > 100) && (value < 15000)) {
|
||||
height_in = value * 0.00034f / 2.0f;
|
||||
height_out = (height_out * (1 - coeff)) + (height_in * coeff);
|
||||
sonardata.Altitude = height_out; // m/us
|
||||
}
|
||||
|
||||
// Update the AltitudeActual UAVObject
|
||||
SonarAltitudeSet(&sonardata);
|
||||
timeout=5;
|
||||
timeout = 5;
|
||||
PIOS_HCSR04_Trigger();
|
||||
}
|
||||
if(!(timeout--))
|
||||
{
|
||||
if(!(timeout--)) {
|
||||
//retrigger
|
||||
timeout=5;
|
||||
timeout = 5;
|
||||
PIOS_HCSR04_Trigger();
|
||||
}
|
||||
}
|
||||
|
||||
@ -90,8 +90,8 @@ static void altitudeTask(void *parameters)
|
||||
|
||||
#if defined(PIOS_INCLUDE_HCSR04)
|
||||
SonarAltitudeData sonardata;
|
||||
int32_t value=0,timeout=10,sample_rate=0;
|
||||
float coeff=0.25,height_out=0,height_in=0;
|
||||
int32_t value = 0, timeout = 10, sample_rate = 0;
|
||||
float coeff = 0.25, height_out = 0, height_in = 0;
|
||||
PIOS_HCSR04_Trigger();
|
||||
#endif
|
||||
|
||||
@ -110,30 +110,27 @@ static void altitudeTask(void *parameters)
|
||||
#if defined(PIOS_INCLUDE_HCSR04)
|
||||
// Compute the current altitude
|
||||
// depends on baro samplerate
|
||||
if(!(sample_rate--))
|
||||
{
|
||||
if(PIOS_HCSR04_Completed())
|
||||
{
|
||||
if(!(sample_rate--)) {
|
||||
if(PIOS_HCSR04_Completed()) {
|
||||
value = PIOS_HCSR04_Get();
|
||||
if((value>100) && (value < 15000)) //from 3.4cm to 5.1m
|
||||
{
|
||||
height_in = value*0.00034f/2.0f;
|
||||
//from 3.4cm to 5.1m
|
||||
if((value > 100) && (value < 15000)) {
|
||||
height_in = value * 0.00034f / 2.0f;
|
||||
height_out = (height_out * (1 - coeff)) + (height_in * coeff);
|
||||
sonardata.Altitude = height_out; // m/us
|
||||
}
|
||||
|
||||
// Update the SonarAltitude UAVObject
|
||||
SonarAltitudeSet(&sonardata);
|
||||
timeout=10;
|
||||
timeout = 10;
|
||||
PIOS_HCSR04_Trigger();
|
||||
}
|
||||
if(!(timeout--))
|
||||
{
|
||||
if(!(timeout--)) {
|
||||
//retrigger
|
||||
timeout=10;
|
||||
timeout = 10;
|
||||
PIOS_HCSR04_Trigger();
|
||||
}
|
||||
sample_rate=25;
|
||||
sample_rate = 25;
|
||||
}
|
||||
#endif
|
||||
float temp, press;
|
||||
|
||||
@ -73,11 +73,7 @@
|
||||
// Private types
|
||||
typedef enum
|
||||
{
|
||||
ARM_STATE_DISARMED,
|
||||
ARM_STATE_ARMING_MANUAL,
|
||||
ARM_STATE_ARMED,
|
||||
ARM_STATE_DISARMING_MANUAL,
|
||||
ARM_STATE_DISARMING_TIMEOUT
|
||||
ARM_STATE_DISARMED, ARM_STATE_ARMING_MANUAL, ARM_STATE_ARMED, ARM_STATE_DISARMING_MANUAL, ARM_STATE_DISARMING_TIMEOUT
|
||||
} ArmState_t;
|
||||
|
||||
// Private variables
|
||||
@ -114,7 +110,8 @@ static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel
|
||||
|
||||
#define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12
|
||||
#define RCVR_ACTIVITY_MONITOR_MIN_RANGE 10
|
||||
struct rcvr_activity_fsm {
|
||||
struct rcvr_activity_fsm
|
||||
{
|
||||
ManualControlSettingsChannelGroupsOptions group;
|
||||
uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP];
|
||||
uint8_t sample_count;
|
||||
@ -132,7 +129,7 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm);
|
||||
int32_t ManualControlStart()
|
||||
{
|
||||
// Start main task
|
||||
xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
|
||||
xTaskCreate(manualControlTask, (signed char *) "ManualControl", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
|
||||
TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle);
|
||||
PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
|
||||
|
||||
@ -146,7 +143,7 @@ int32_t ManualControlInitialize()
|
||||
{
|
||||
|
||||
/* Check the assumptions about uavobject enum's are correct */
|
||||
if(!assumptions)
|
||||
if (!assumptions)
|
||||
return -1;
|
||||
|
||||
AccessoryDesiredInitialize();
|
||||
@ -158,7 +155,7 @@ int32_t ManualControlInitialize()
|
||||
|
||||
return 0;
|
||||
}
|
||||
MODULE_INITCALL(ManualControlInitialize, ManualControlStart)
|
||||
MODULE_INITCALL( ManualControlInitialize, ManualControlStart)
|
||||
|
||||
/**
|
||||
* Module task
|
||||
@ -224,7 +221,7 @@ static void manualControlTask(void *parameters)
|
||||
if (ManualControlCommandReadOnly()) {
|
||||
FlightTelemetryStatsData flightTelemStats;
|
||||
FlightTelemetryStatsGet(&flightTelemStats);
|
||||
if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
|
||||
if (flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
|
||||
/* trying to fly via GCS and lost connection. fall back to transmitter */
|
||||
UAVObjMetadata metadata;
|
||||
ManualControlCommandGetMetadata(&metadata);
|
||||
@ -238,48 +235,47 @@ static void manualControlTask(void *parameters)
|
||||
bool valid_input_detected = true;
|
||||
|
||||
// Read channel values in us
|
||||
for (uint8_t n = 0;
|
||||
n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM;
|
||||
++n) {
|
||||
for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) {
|
||||
extern uint32_t pios_rcvr_group_map[];
|
||||
|
||||
if (settings.ChannelGroups[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
|
||||
cmd.Channel[n] = PIOS_RCVR_INVALID;
|
||||
} else {
|
||||
cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[settings.ChannelGroups[n]],
|
||||
settings.ChannelNumber[n]);
|
||||
cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[settings.ChannelGroups[n]], settings.ChannelNumber[n]);
|
||||
}
|
||||
|
||||
// If a channel has timed out this is not valid data and we shouldn't update anything
|
||||
// until we decide to go to failsafe
|
||||
if(cmd.Channel[n] == PIOS_RCVR_TIMEOUT)
|
||||
if (cmd.Channel[n] == PIOS_RCVR_TIMEOUT)
|
||||
valid_input_detected = false;
|
||||
else
|
||||
scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]);
|
||||
}
|
||||
|
||||
// Check settings, if error raise alarm
|
||||
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
|
||||
settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
|
||||
settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
|
||||
settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
|
||||
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|
||||
|| settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|
||||
|| settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|
||||
|| settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|
||||
||
|
||||
// Check all channel mappings are valid
|
||||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_INVALID ||
|
||||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_INVALID ||
|
||||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_INVALID ||
|
||||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_INVALID ||
|
||||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_INVALID
|
||||
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_INVALID
|
||||
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_INVALID
|
||||
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_INVALID
|
||||
||
|
||||
// Check the driver exists
|
||||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_NODRIVER ||
|
||||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_NODRIVER ||
|
||||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_NODRIVER ||
|
||||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_NODRIVER ||
|
||||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_NODRIVER
|
||||
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_NODRIVER
|
||||
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_NODRIVER
|
||||
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_NODRIVER ||
|
||||
// Check the FlightModeNumber is valid
|
||||
settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM ||
|
||||
// Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care
|
||||
((settings.FlightModeNumber > 1) && (
|
||||
settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
|
||||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_INVALID ||
|
||||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_NODRIVER))) {
|
||||
((settings.FlightModeNumber > 1)
|
||||
&& (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|
||||
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_INVALID
|
||||
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_NODRIVER))) {
|
||||
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
|
||||
@ -293,10 +289,14 @@ static void manualControlTask(void *parameters)
|
||||
}
|
||||
|
||||
// decide if we have valid manual input or not
|
||||
valid_input_detected &= validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) &&
|
||||
validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) &&
|
||||
validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) &&
|
||||
validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]);
|
||||
valid_input_detected &= validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE],
|
||||
settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE])
|
||||
&& validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL],
|
||||
settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL])
|
||||
&& validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW],
|
||||
settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW])
|
||||
&& validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH],
|
||||
settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]);
|
||||
|
||||
// Implement hysteresis loop on connection status
|
||||
if (valid_input_detected && (++connected_count > 10)) {
|
||||
@ -322,24 +322,21 @@ static void manualControlTask(void *parameters)
|
||||
|
||||
AccessoryDesiredData accessory;
|
||||
// Set Accessory 0
|
||||
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] !=
|
||||
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
|
||||
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
|
||||
accessory.AccessoryVal = 0;
|
||||
if(AccessoryDesiredInstSet(0, &accessory) != 0)
|
||||
if (AccessoryDesiredInstSet(0, &accessory) != 0)
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
|
||||
}
|
||||
// Set Accessory 1
|
||||
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] !=
|
||||
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
|
||||
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
|
||||
accessory.AccessoryVal = 0;
|
||||
if(AccessoryDesiredInstSet(1, &accessory) != 0)
|
||||
if (AccessoryDesiredInstSet(1, &accessory) != 0)
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
|
||||
}
|
||||
// Set Accessory 2
|
||||
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] !=
|
||||
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
|
||||
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
|
||||
accessory.AccessoryVal = 0;
|
||||
if(AccessoryDesiredInstSet(2, &accessory) != 0)
|
||||
if (AccessoryDesiredInstSet(2, &accessory) != 0)
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
|
||||
}
|
||||
|
||||
@ -371,40 +368,37 @@ static void manualControlTask(void *parameters)
|
||||
applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings, dT);
|
||||
applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings, dT);
|
||||
#endif // USE_INPUT_LPF
|
||||
if(cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_INVALID &&
|
||||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_NODRIVER &&
|
||||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_TIMEOUT)
|
||||
if (cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_INVALID
|
||||
&& cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_NODRIVER
|
||||
&& cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_TIMEOUT)
|
||||
cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE];
|
||||
|
||||
AccessoryDesiredData accessory;
|
||||
// Set Accessory 0
|
||||
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] !=
|
||||
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
|
||||
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
|
||||
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0];
|
||||
#ifdef USE_INPUT_LPF
|
||||
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT);
|
||||
#endif
|
||||
if(AccessoryDesiredInstSet(0, &accessory) != 0)
|
||||
if (AccessoryDesiredInstSet(0, &accessory) != 0)
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
|
||||
}
|
||||
// Set Accessory 1
|
||||
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] !=
|
||||
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
|
||||
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
|
||||
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1];
|
||||
#ifdef USE_INPUT_LPF
|
||||
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT);
|
||||
#endif
|
||||
if(AccessoryDesiredInstSet(1, &accessory) != 0)
|
||||
if (AccessoryDesiredInstSet(1, &accessory) != 0)
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
|
||||
}
|
||||
// Set Accessory 2
|
||||
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] !=
|
||||
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
|
||||
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
|
||||
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2];
|
||||
#ifdef USE_INPUT_LPF
|
||||
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT);
|
||||
#endif
|
||||
if(AccessoryDesiredInstSet(2, &accessory) != 0)
|
||||
if (AccessoryDesiredInstSet(2, &accessory) != 0)
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
|
||||
}
|
||||
|
||||
@ -435,7 +429,7 @@ static void manualControlTask(void *parameters)
|
||||
|
||||
// Depending on the mode update the Stabilization or Actuator objects
|
||||
static uint8_t lastFlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL;
|
||||
switch(PARSE_FLIGHT_MODE(flightStatus.FlightMode)) {
|
||||
switch (PARSE_FLIGHT_MODE(flightStatus.FlightMode)) {
|
||||
case FLIGHTMODE_UNDEFINED:
|
||||
// This reflects a bug in the code architecture!
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
@ -451,7 +445,7 @@ static void manualControlTask(void *parameters)
|
||||
// call anything else. This just avoids errors.
|
||||
break;
|
||||
case FLIGHTMODE_GUIDANCE:
|
||||
switch(flightStatus.FlightMode) {
|
||||
switch (flightStatus.FlightMode) {
|
||||
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
|
||||
altitudeHoldDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
|
||||
break;
|
||||
@ -484,8 +478,7 @@ static void resetRcvrActivity(struct rcvr_activity_fsm * fsm)
|
||||
|
||||
/* Clear all channel activity flags */
|
||||
ReceiverActivityGet(&data);
|
||||
if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE &&
|
||||
data.ActiveChannel != 255) {
|
||||
if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE && data.ActiveChannel != 255) {
|
||||
data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE;
|
||||
data.ActiveChannel = 255;
|
||||
updated = true;
|
||||
@ -499,7 +492,8 @@ static void resetRcvrActivity(struct rcvr_activity_fsm * fsm)
|
||||
fsm->sample_count = 0;
|
||||
}
|
||||
|
||||
static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8_t max_channels) {
|
||||
static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8_t max_channels)
|
||||
{
|
||||
for (uint8_t channel = 1; channel <= max_channels; channel++) {
|
||||
// Subtract 1 because channels are 1 indexed
|
||||
samples[channel - 1] = PIOS_RCVR_Read(rcvr_id, channel);
|
||||
@ -511,9 +505,7 @@ static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm
|
||||
bool activity_updated = false;
|
||||
|
||||
/* Compare the current value to the previous sampled value */
|
||||
for (uint8_t channel = 1;
|
||||
channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP;
|
||||
channel++) {
|
||||
for (uint8_t channel = 1; channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; channel++) {
|
||||
uint16_t delta;
|
||||
uint16_t prev = fsm->prev[channel - 1]; // Subtract 1 because channels are 1 indexed
|
||||
uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel);
|
||||
@ -555,7 +547,7 @@ static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm
|
||||
break;
|
||||
}
|
||||
|
||||
ReceiverActivityActiveGroupSet((uint8_t*)&group);
|
||||
ReceiverActivityActiveGroupSet((uint8_t*) &group);
|
||||
ReceiverActivityActiveChannelSet(&channel);
|
||||
activity_updated = true;
|
||||
}
|
||||
@ -580,9 +572,7 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm)
|
||||
|
||||
if (fsm->sample_count == 0) {
|
||||
/* Take a sample of each channel in this group */
|
||||
updateRcvrActivitySample(pios_rcvr_group_map[fsm->group],
|
||||
fsm->prev,
|
||||
NELEMENTS(fsm->prev));
|
||||
updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev));
|
||||
fsm->sample_count++;
|
||||
return (false);
|
||||
}
|
||||
@ -590,7 +580,7 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm)
|
||||
/* Compare with previous sample */
|
||||
activity_updated = updateRcvrActivityCompare(pios_rcvr_group_map[fsm->group], fsm);
|
||||
|
||||
group_completed:
|
||||
group_completed:
|
||||
/* Reset the sample counter */
|
||||
fsm->sample_count = 0;
|
||||
|
||||
@ -608,9 +598,7 @@ group_completed:
|
||||
* extra 20ms delay in the main thread so we can speed up
|
||||
* this algorithm.
|
||||
*/
|
||||
updateRcvrActivitySample(pios_rcvr_group_map[fsm->group],
|
||||
fsm->prev,
|
||||
NELEMENTS(fsm->prev));
|
||||
updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev));
|
||||
fsm->sample_count++;
|
||||
break;
|
||||
}
|
||||
@ -641,7 +629,7 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
|
||||
uint8_t * stab_settings;
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
switch(flightStatus.FlightMode) {
|
||||
switch (flightStatus.FlightMode) {
|
||||
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
|
||||
stab_settings = settings->Stabilization1Settings;
|
||||
break;
|
||||
@ -662,35 +650,41 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
|
||||
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1];
|
||||
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2];
|
||||
|
||||
stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll :
|
||||
stabilization.Roll =
|
||||
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll :
|
||||
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
|
||||
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
|
||||
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
|
||||
cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
|
||||
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
|
||||
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
|
||||
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll :
|
||||
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
|
||||
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax :
|
||||
0; // this is an invalid mode
|
||||
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ?
|
||||
cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
|
||||
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode
|
||||
;
|
||||
stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
|
||||
stabilization.Pitch =
|
||||
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
|
||||
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
|
||||
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
|
||||
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
|
||||
cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
|
||||
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
|
||||
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
|
||||
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ?
|
||||
cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
|
||||
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch :
|
||||
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
|
||||
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
|
||||
0; // this is an invalid mode
|
||||
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ?
|
||||
cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
|
||||
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : 0; // this is an invalid mode
|
||||
|
||||
stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
|
||||
stabilization.Yaw =
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
|
||||
cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw :
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
|
||||
0; // this is an invalid mode
|
||||
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : 0; // this is an invalid mode
|
||||
|
||||
stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
|
||||
StabilizationDesiredSet(&stabilization);
|
||||
@ -755,7 +749,6 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static void updateLandDesired(ManualControlCommandData * cmd, bool changed)
|
||||
{
|
||||
static portTickType lastSysTime;
|
||||
@ -821,12 +814,14 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
|
||||
// After not being in this mode for a while init at current height
|
||||
altitudeHoldDesired.Altitude = 0;
|
||||
zeroed = false;
|
||||
} else if (cmd->Throttle > DEADBAND_HIGH && zeroed)
|
||||
} else if (cmd->Throttle > DEADBAND_HIGH && zeroed) {
|
||||
altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_HIGH) * dT;
|
||||
else if (cmd->Throttle < DEADBAND_LOW && zeroed)
|
||||
} else if (cmd->Throttle < DEADBAND_LOW && zeroed) {
|
||||
altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_LOW) * dT;
|
||||
else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH) // Require the stick to enter the dead band before they can move height
|
||||
} else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH) {
|
||||
// Require the stick to enter the dead band before they can move height
|
||||
zeroed = true;
|
||||
}
|
||||
|
||||
AltitudeHoldDesiredSet(&altitudeHoldDesired);
|
||||
}
|
||||
@ -858,33 +853,36 @@ static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutr
|
||||
float valueScaled;
|
||||
|
||||
// Scale
|
||||
if ((max > min && value >= neutral) || (min > max && value <= neutral))
|
||||
{
|
||||
if (max != neutral)
|
||||
valueScaled = (float)(value - neutral) / (float)(max - neutral);
|
||||
else
|
||||
if ((max > min && value >= neutral) || (min > max && value <= neutral)) {
|
||||
if (max != neutral) {
|
||||
valueScaled = (float) (value - neutral) / (float) (max - neutral);
|
||||
} else {
|
||||
valueScaled = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (min != neutral)
|
||||
valueScaled = (float)(value - neutral) / (float)(neutral - min);
|
||||
else
|
||||
} else {
|
||||
if (min != neutral) {
|
||||
valueScaled = (float) (value - neutral) / (float) (neutral - min);
|
||||
} else {
|
||||
valueScaled = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// Bound
|
||||
if (valueScaled > 1.0) valueScaled = 1.0;
|
||||
else
|
||||
if (valueScaled < -1.0) valueScaled = -1.0;
|
||||
if (valueScaled > 1.0) {
|
||||
valueScaled = 1.0;
|
||||
} else if (valueScaled < -1.0) {
|
||||
valueScaled = -1.0;
|
||||
}
|
||||
|
||||
return valueScaled;
|
||||
}
|
||||
|
||||
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) {
|
||||
if(end_time > start_time)
|
||||
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time)
|
||||
{
|
||||
if (end_time > start_time) {
|
||||
return (end_time - start_time) * portTICK_RATE_MS;
|
||||
return ((((portTICK_RATE_MS) -1) - start_time) + end_time) * portTICK_RATE_MS;
|
||||
}
|
||||
return ((((portTICK_RATE_MS) - 1) - start_time) + end_time) * portTICK_RATE_MS;
|
||||
}
|
||||
|
||||
/**
|
||||
@ -897,12 +895,9 @@ static bool okToArm(void)
|
||||
SystemAlarmsData alarms;
|
||||
SystemAlarmsGet(&alarms);
|
||||
|
||||
|
||||
// Check each alarm
|
||||
for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++)
|
||||
{
|
||||
if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR)
|
||||
{ // found an alarm thats set
|
||||
for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) {
|
||||
if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set
|
||||
if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY)
|
||||
continue;
|
||||
|
||||
@ -932,11 +927,12 @@ static bool forcedDisArm(void)
|
||||
* @brief Update the flightStatus object only if value changed. Reduces callbacks
|
||||
* @param[in] val The new value
|
||||
*/
|
||||
static void setArmedIfChanged(uint8_t val) {
|
||||
static void setArmedIfChanged(uint8_t val)
|
||||
{
|
||||
FlightStatusData flightStatus;
|
||||
FlightStatusGet(&flightStatus);
|
||||
|
||||
if(flightStatus.Armed != val) {
|
||||
if (flightStatus.Armed != val) {
|
||||
flightStatus.Armed = val;
|
||||
FlightStatusSet(&flightStatus);
|
||||
}
|
||||
@ -968,7 +964,7 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
|
||||
|
||||
// The throttle is not low, in case we where arming or disarming, abort
|
||||
if (!lowThrottle) {
|
||||
switch(armState) {
|
||||
switch (armState) {
|
||||
case ARM_STATE_DISARMING_MANUAL:
|
||||
case ARM_STATE_DISARMING_TIMEOUT:
|
||||
armState = ARM_STATE_ARMED;
|
||||
@ -990,18 +986,23 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
// When the configuration is not "Always armed" and no "Always disarmed",
|
||||
// the state will not be changed when the throttle is not low
|
||||
static portTickType armedDisarmStart;
|
||||
float armingInputLevel = 0;
|
||||
|
||||
// Calc channel see assumptions7
|
||||
int8_t sign = ((settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) ? -1 : 1;
|
||||
switch ( (settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) {
|
||||
case ARMING_CHANNEL_ROLL: armingInputLevel = sign * cmd->Roll; break;
|
||||
case ARMING_CHANNEL_PITCH: armingInputLevel = sign * cmd->Pitch; break;
|
||||
case ARMING_CHANNEL_YAW: armingInputLevel = sign * cmd->Yaw; break;
|
||||
int8_t sign = ((settings->Arming - MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2) ? -1 : 1;
|
||||
switch ((settings->Arming - MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2) {
|
||||
case ARMING_CHANNEL_ROLL:
|
||||
armingInputLevel = sign * cmd->Roll;
|
||||
break;
|
||||
case ARMING_CHANNEL_PITCH:
|
||||
armingInputLevel = sign * cmd->Pitch;
|
||||
break;
|
||||
case ARMING_CHANNEL_YAW:
|
||||
armingInputLevel = sign * cmd->Yaw;
|
||||
break;
|
||||
}
|
||||
|
||||
bool manualArm = false;
|
||||
@ -1012,7 +1013,7 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
|
||||
else if (armingInputLevel >= +ARMED_THRESHOLD)
|
||||
manualDisarm = true;
|
||||
|
||||
switch(armState) {
|
||||
switch (armState) {
|
||||
case ARM_STATE_DISARMED:
|
||||
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
|
||||
|
||||
@ -1053,7 +1054,7 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
|
||||
break;
|
||||
|
||||
case ARM_STATE_DISARMING_MANUAL:
|
||||
if (manualDisarm &&(timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
|
||||
if (manualDisarm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
|
||||
armState = ARM_STATE_DISARMED;
|
||||
else if (!manualDisarm)
|
||||
armState = ARM_STATE_ARMED;
|
||||
@ -1092,8 +1093,7 @@ static void processFlightMode(ManualControlSettingsData *settings, float flightM
|
||||
*/
|
||||
bool validInputRange(int16_t min, int16_t max, uint16_t value)
|
||||
{
|
||||
if (min > max)
|
||||
{
|
||||
if (min > max) {
|
||||
int16_t tmp = min;
|
||||
min = max;
|
||||
max = tmp;
|
||||
@ -1108,8 +1108,7 @@ static void applyDeadband(float *value, float deadband)
|
||||
{
|
||||
if (fabs(*value) < deadband)
|
||||
*value = 0.0f;
|
||||
else
|
||||
if (*value > 0.0f)
|
||||
else if (*value > 0.0f)
|
||||
*value -= deadband;
|
||||
else
|
||||
*value += deadband;
|
||||
@ -1128,7 +1127,6 @@ static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel
|
||||
}
|
||||
}
|
||||
#endif // USE_INPUT_LPF
|
||||
|
||||
/**
|
||||
* Called whenever a critical configuration component changes
|
||||
*/
|
||||
@ -1137,8 +1135,6 @@ static void configurationUpdatedCb(UAVObjEvent * ev)
|
||||
configuration_check();
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
|
||||
@ -29,7 +29,6 @@
|
||||
*/
|
||||
|
||||
// ****************
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "osdgen.h"
|
||||
#include "attitudeactual.h"
|
||||
@ -47,29 +46,28 @@
|
||||
|
||||
#include "splash.h"
|
||||
/*
|
||||
static uint16_t angleA=0;
|
||||
static int16_t angleB=90;
|
||||
static int16_t angleC=0;
|
||||
static int16_t sum=2;
|
||||
static uint16_t angleA=0;
|
||||
static int16_t angleB=90;
|
||||
static int16_t angleC=0;
|
||||
static int16_t sum=2;
|
||||
|
||||
static int16_t m_pitch=0;
|
||||
static int16_t m_roll=0;
|
||||
static int16_t m_yaw=0;
|
||||
static int16_t m_batt=0;
|
||||
static int16_t m_alt=0;
|
||||
static int16_t m_pitch=0;
|
||||
static int16_t m_roll=0;
|
||||
static int16_t m_yaw=0;
|
||||
static int16_t m_batt=0;
|
||||
static int16_t m_alt=0;
|
||||
|
||||
static uint8_t m_gpsStatus=0;
|
||||
static int32_t m_gpsLat=0;
|
||||
static int32_t m_gpsLon=0;
|
||||
static float m_gpsAlt=0;
|
||||
static float m_gpsSpd=0;*/
|
||||
static uint8_t m_gpsStatus=0;
|
||||
static int32_t m_gpsLat=0;
|
||||
static int32_t m_gpsLon=0;
|
||||
static float m_gpsAlt=0;
|
||||
static float m_gpsSpd=0;*/
|
||||
|
||||
extern uint8_t *draw_buffer_level;
|
||||
extern uint8_t *draw_buffer_mask;
|
||||
extern uint8_t *disp_buffer_level;
|
||||
extern uint8_t *disp_buffer_mask;
|
||||
|
||||
|
||||
TTime timex;
|
||||
|
||||
// ****************
|
||||
@ -99,88 +97,81 @@ struct splashEntry
|
||||
const uint16_t *mask;
|
||||
};
|
||||
|
||||
struct splashEntry splash[3] = {
|
||||
{ oplogo_width,
|
||||
oplogo_height,
|
||||
oplogo_bits,
|
||||
oplogo_mask_bits },
|
||||
{ level_width,
|
||||
level_height,
|
||||
level_bits,
|
||||
level_mask_bits },
|
||||
{ llama_width,
|
||||
llama_height,
|
||||
llama_bits,
|
||||
llama_mask_bits },
|
||||
struct splashEntry splash[3] =
|
||||
{
|
||||
{ oplogo_width, oplogo_height, oplogo_bits, oplogo_mask_bits },
|
||||
{ level_width, level_height, level_bits, level_mask_bits },
|
||||
{ llama_width, llama_height, llama_bits, llama_mask_bits },
|
||||
|
||||
};
|
||||
|
||||
uint16_t mirror(uint16_t source)
|
||||
{
|
||||
int result = ((source & 0x8000) >> 7) | ((source & 0x4000) >> 5) |
|
||||
((source & 0x2000) >> 3) | ((source & 0x1000) >> 1) |
|
||||
((source & 0x0800) << 1) | ((source & 0x0400) << 3) |
|
||||
((source & 0x0200) << 5) | ((source & 0x0100) << 7) |
|
||||
((source & 0x0080) >> 7) | ((source & 0x0040) >> 5) |
|
||||
((source & 0x0020) >> 3) | ((source & 0x0010) >> 1) |
|
||||
((source & 0x0008) << 1) | ((source & 0x0004) << 3) |
|
||||
((source & 0x0002) << 5) | ((source & 0x0001) << 7);
|
||||
int result = ((source & 0x8000) >> 7) | ((source & 0x4000) >> 5) | ((source & 0x2000) >> 3) | ((source & 0x1000) >> 1) | ((source & 0x0800) << 1)
|
||||
| ((source & 0x0400) << 3) | ((source & 0x0200) << 5) | ((source & 0x0100) << 7) | ((source & 0x0080) >> 7) | ((source & 0x0040) >> 5)
|
||||
| ((source & 0x0020) >> 3) | ((source & 0x0010) >> 1) | ((source & 0x0008) << 1) | ((source & 0x0004) << 3) | ((source & 0x0002) << 5)
|
||||
| ((source & 0x0001) << 7);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
void clearGraphics() {
|
||||
void clearGraphics()
|
||||
{
|
||||
memset((uint8_t *) draw_buffer_mask, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT);
|
||||
memset((uint8_t *) draw_buffer_level, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT);
|
||||
}
|
||||
|
||||
void copyimage(uint16_t offsetx, uint16_t offsety, int image) {
|
||||
void copyimage(uint16_t offsetx, uint16_t offsety, int image)
|
||||
{
|
||||
//check top/left position
|
||||
if (!validPos(offsetx, offsety)) {
|
||||
return;
|
||||
}
|
||||
struct splashEntry splash_info;
|
||||
splash_info = splash[image];
|
||||
offsetx=offsetx/8;
|
||||
for (uint16_t y = offsety; y < ((splash_info.height)+offsety); y++) {
|
||||
uint16_t x1=offsetx;
|
||||
for (uint16_t x = offsetx; x < (((splash_info.width)/16)+offsetx); x++) {
|
||||
draw_buffer_level[y*GRAPHICS_WIDTH+x1+1] = (uint8_t)(mirror(splash_info.level[(y-offsety)*((splash_info.width)/16)+(x-offsetx)])>>8);
|
||||
draw_buffer_level[y*GRAPHICS_WIDTH+x1] = (uint8_t)(mirror(splash_info.level[(y-offsety)*((splash_info.width)/16)+(x-offsetx)])&0xFF);
|
||||
draw_buffer_mask[y*GRAPHICS_WIDTH+x1+1] = (uint8_t)(mirror(splash_info.mask[(y-offsety)*((splash_info.width)/16)+(x-offsetx)])>>8);
|
||||
draw_buffer_mask[y*GRAPHICS_WIDTH+x1] = (uint8_t)(mirror(splash_info.mask[(y-offsety)*((splash_info.width)/16)+(x-offsetx)])&0xFF);
|
||||
x1+=2;
|
||||
offsetx = offsetx / 8;
|
||||
for (uint16_t y = offsety; y < ((splash_info.height) + offsety); y++) {
|
||||
uint16_t x1 = offsetx;
|
||||
for (uint16_t x = offsetx; x < (((splash_info.width) / 16) + offsetx); x++) {
|
||||
draw_buffer_level[y * GRAPHICS_WIDTH + x1 + 1] = (uint8_t)(
|
||||
mirror(splash_info.level[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) >> 8);
|
||||
draw_buffer_level[y * GRAPHICS_WIDTH + x1] = (uint8_t)(
|
||||
mirror(splash_info.level[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) & 0xFF);
|
||||
draw_buffer_mask[y * GRAPHICS_WIDTH + x1 + 1] = (uint8_t)(
|
||||
mirror(splash_info.mask[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) >> 8);
|
||||
draw_buffer_mask[y * GRAPHICS_WIDTH + x1] = (uint8_t)(mirror(splash_info.mask[(y - offsety) * ((splash_info.width) / 16) + (x - offsetx)]) & 0xFF);
|
||||
x1 += 2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t validPos(uint16_t x, uint16_t y) {
|
||||
if ( x < GRAPHICS_HDEADBAND || x >= GRAPHICS_WIDTH_REAL || y >= GRAPHICS_HEIGHT_REAL) {
|
||||
uint8_t validPos(uint16_t x, uint16_t y)
|
||||
{
|
||||
if (x < GRAPHICS_HDEADBAND || x >= GRAPHICS_WIDTH_REAL || y >= GRAPHICS_HEIGHT_REAL) {
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Credit for this one goes to wikipedia! :-)
|
||||
void drawCircle(uint16_t x0, uint16_t y0, uint16_t radius) {
|
||||
void drawCircle(uint16_t x0, uint16_t y0, uint16_t radius)
|
||||
{
|
||||
int f = 1 - radius;
|
||||
int ddF_x = 1;
|
||||
int ddF_y = -2 * radius;
|
||||
int x = 0;
|
||||
int y = radius;
|
||||
|
||||
write_pixel_lm(x0, y0 + radius,1,1);
|
||||
write_pixel_lm(x0, y0 - radius,1,1);
|
||||
write_pixel_lm(x0 + radius, y0,1,1);
|
||||
write_pixel_lm(x0 - radius, y0,1,1);
|
||||
write_pixel_lm(x0, y0 + radius, 1, 1);
|
||||
write_pixel_lm(x0, y0 - radius, 1, 1);
|
||||
write_pixel_lm(x0 + radius, y0, 1, 1);
|
||||
write_pixel_lm(x0 - radius, y0, 1, 1);
|
||||
|
||||
while(x < y)
|
||||
{
|
||||
while (x < y) {
|
||||
// ddF_x == 2 * x + 1;
|
||||
// ddF_y == -2 * y;
|
||||
// f == x*x + y*y - radius*radius + 2*x - y + 1;
|
||||
if(f >= 0)
|
||||
{
|
||||
if (f >= 0) {
|
||||
y--;
|
||||
ddF_y += 2;
|
||||
f += ddF_y;
|
||||
@ -188,32 +179,31 @@ void drawCircle(uint16_t x0, uint16_t y0, uint16_t radius) {
|
||||
x++;
|
||||
ddF_x += 2;
|
||||
f += ddF_x;
|
||||
write_pixel_lm(x0 + x, y0 + y,1,1);
|
||||
write_pixel_lm(x0 - x, y0 + y,1,1);
|
||||
write_pixel_lm(x0 + x, y0 - y,1,1);
|
||||
write_pixel_lm(x0 - x, y0 - y,1,1);
|
||||
write_pixel_lm(x0 + y, y0 + x,1,1);
|
||||
write_pixel_lm(x0 - y, y0 + x,1,1);
|
||||
write_pixel_lm(x0 + y, y0 - x,1,1);
|
||||
write_pixel_lm(x0 - y, y0 - x,1,1);
|
||||
write_pixel_lm(x0 + x, y0 + y, 1, 1);
|
||||
write_pixel_lm(x0 - x, y0 + y, 1, 1);
|
||||
write_pixel_lm(x0 + x, y0 - y, 1, 1);
|
||||
write_pixel_lm(x0 - x, y0 - y, 1, 1);
|
||||
write_pixel_lm(x0 + y, y0 + x, 1, 1);
|
||||
write_pixel_lm(x0 - y, y0 + x, 1, 1);
|
||||
write_pixel_lm(x0 + y, y0 - x, 1, 1);
|
||||
write_pixel_lm(x0 - y, y0 - x, 1, 1);
|
||||
}
|
||||
}
|
||||
|
||||
void swap(uint16_t* a, uint16_t* b) {
|
||||
void swap(uint16_t* a, uint16_t* b)
|
||||
{
|
||||
uint16_t temp = *a;
|
||||
*a = *b;
|
||||
*b = temp;
|
||||
}
|
||||
|
||||
const static int8_t sinData[91] =
|
||||
{ 0, 2, 3, 5, 7, 9, 10, 12, 14, 16, 17, 19, 21, 22, 24, 26, 28, 29, 31, 33, 34, 36, 37, 39, 41, 42, 44, 45, 47, 48, 50, 52, 53, 54, 56, 57, 59, 60, 62, 63, 64,
|
||||
66, 67, 68, 69, 71, 72, 73, 74, 75, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 87, 88, 89, 90, 91, 91, 92, 93, 93, 94, 95, 95, 96, 96, 97, 97, 97, 98,
|
||||
98, 98, 99, 99, 99, 99, 100, 100, 100, 100, 100, 100 };
|
||||
|
||||
const static int8_t sinData[91] = {
|
||||
0, 2, 3, 5, 7, 9, 10, 12, 14, 16, 17, 19, 21, 22, 24, 26, 28, 29, 31, 33,
|
||||
34, 36, 37, 39, 41, 42, 44, 45, 47, 48, 50, 52, 53, 54, 56, 57, 59, 60, 62,
|
||||
63, 64, 66, 67, 68, 69, 71, 72, 73, 74, 75, 77, 78, 79, 80, 81, 82, 83, 84,
|
||||
85, 86, 87, 87, 88, 89, 90, 91, 91, 92, 93, 93, 94, 95, 95, 96, 96, 97, 97,
|
||||
97, 98, 98, 98, 99, 99, 99, 99, 100, 100, 100, 100, 100, 100};
|
||||
|
||||
static int8_t mySin(uint16_t angle) {
|
||||
static int8_t mySin(uint16_t angle)
|
||||
{
|
||||
uint16_t pos = 0;
|
||||
pos = angle % 360;
|
||||
int8_t mult = 1;
|
||||
@ -229,7 +219,8 @@ static int8_t mySin(uint16_t angle) {
|
||||
return mult * (int8_t)(sinData[pos]);
|
||||
}
|
||||
|
||||
static int8_t myCos(uint16_t angle) {
|
||||
static int8_t myCos(uint16_t angle)
|
||||
{
|
||||
return mySin(angle + 90);
|
||||
}
|
||||
|
||||
@ -247,10 +238,10 @@ static int8_t myCos(uint16_t angle) {
|
||||
/// \param color the color to draw the pixels with.
|
||||
void plotFourQuadrants(int32_t centerX, int32_t centerY, int32_t deltaX, int32_t deltaY)
|
||||
{
|
||||
write_pixel_lm(centerX + deltaX, centerY + deltaY,1,1); // Ist Quadrant
|
||||
write_pixel_lm(centerX - deltaX, centerY + deltaY,1,1); // IInd Quadrant
|
||||
write_pixel_lm(centerX - deltaX, centerY - deltaY,1,1); // IIIrd Quadrant
|
||||
write_pixel_lm(centerX + deltaX, centerY - deltaY,1,1); // IVth Quadrant
|
||||
write_pixel_lm(centerX + deltaX, centerY + deltaY, 1, 1); // Ist Quadrant
|
||||
write_pixel_lm(centerX - deltaX, centerY + deltaY, 1, 1); // IInd Quadrant
|
||||
write_pixel_lm(centerX - deltaX, centerY - deltaY, 1, 1); // IIIrd Quadrant
|
||||
write_pixel_lm(centerX + deltaX, centerY - deltaY, 1, 1); // IVth Quadrant
|
||||
}
|
||||
|
||||
/// Implements the midpoint ellipse drawing algorithm which is a bresenham
|
||||
@ -275,15 +266,13 @@ void ellipse(int centerX, int centerY, int horizontalRadius, int verticalRadius)
|
||||
|
||||
plotFourQuadrants(centerX, centerY, x, y);
|
||||
|
||||
while(deltaY >= deltaX)
|
||||
{
|
||||
while (deltaY >= deltaX) {
|
||||
x++;
|
||||
deltaX += (doubleVerticalRadius << 1);
|
||||
|
||||
error += deltaX + doubleVerticalRadius;
|
||||
|
||||
if(error >= 0)
|
||||
{
|
||||
if (error >= 0) {
|
||||
y--;
|
||||
deltaY -= (doubleHorizontalRadius << 1);
|
||||
|
||||
@ -292,17 +281,16 @@ void ellipse(int centerX, int centerY, int horizontalRadius, int verticalRadius)
|
||||
plotFourQuadrants(centerX, centerY, x, y);
|
||||
}
|
||||
|
||||
error = (int64_t)(doubleVerticalRadius * (x + 1 / 2.0) * (x + 1 / 2.0) + doubleHorizontalRadius * (y - 1) * (y - 1) - doubleHorizontalRadius * doubleVerticalRadius);
|
||||
error = (int64_t)(
|
||||
doubleVerticalRadius * (x + 1 / 2.0) * (x + 1 / 2.0) + doubleHorizontalRadius * (y - 1) * (y - 1) - doubleHorizontalRadius * doubleVerticalRadius);
|
||||
|
||||
while (y>=0)
|
||||
{
|
||||
while (y >= 0) {
|
||||
error += doubleHorizontalRadius;
|
||||
y--;
|
||||
deltaY -= (doubleHorizontalRadius<<1);
|
||||
deltaY -= (doubleHorizontalRadius << 1);
|
||||
error -= deltaY;
|
||||
|
||||
if(error <= 0)
|
||||
{
|
||||
if (error <= 0) {
|
||||
x++;
|
||||
deltaX += (doubleVerticalRadius << 1);
|
||||
error += deltaX;
|
||||
@ -312,17 +300,16 @@ void ellipse(int centerX, int centerY, int horizontalRadius, int verticalRadius)
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void drawArrow(uint16_t x, uint16_t y, uint16_t angle, uint16_t size)
|
||||
{
|
||||
int16_t a = myCos(angle);
|
||||
int16_t b = mySin(angle);
|
||||
a = (a * (size/2)) / 100;
|
||||
b = (b * (size/2)) / 100;
|
||||
write_line_lm((x)-1 - b, (y)-1 + a, (x)-1 + b, (y)-1 - a, 1, 1); //Direction line
|
||||
a = (a * (size / 2)) / 100;
|
||||
b = (b * (size / 2)) / 100;
|
||||
write_line_lm((x) - 1 - b, (y) - 1 + a, (x) - 1 + b, (y) - 1 - a, 1, 1); //Direction line
|
||||
//write_line_lm((GRAPHICS_SIZE/2)-1 + a/2, (GRAPHICS_SIZE/2)-1 + b/2, (GRAPHICS_SIZE/2)-1 - a/2, (GRAPHICS_SIZE/2)-1 - b/2, 1, 1); //Arrow bottom line
|
||||
write_line_lm((x)-1 + b, (y)-1 - a, (x)-1 - a/2, (y)-1 - b/2, 1, 1); // Arrow "wings"
|
||||
write_line_lm((x)-1 + b, (y)-1 - a, (x)-1 + a/2, (y)-1 + b/2, 1, 1);
|
||||
write_line_lm((x) - 1 + b, (y) - 1 - a, (x) - 1 - a / 2, (y) - 1 - b / 2, 1, 1); // Arrow "wings"
|
||||
write_line_lm((x) - 1 + b, (y) - 1 - a, (x) - 1 + a / 2, (y) - 1 + b / 2, 1, 1);
|
||||
}
|
||||
|
||||
void drawBox(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2)
|
||||
@ -379,7 +366,6 @@ void write_pixel_lm(unsigned int x, unsigned int y, int mmode, int lmode)
|
||||
WRITE_WORD_MODE(draw_buffer_level, wordnum, mask, lmode);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* write_hline: optimised horizontal line writing algorithm
|
||||
*
|
||||
@ -393,11 +379,12 @@ void write_hline(uint8_t *buff, unsigned int x0, unsigned int x1, unsigned int y
|
||||
{
|
||||
CLIP_COORDS(x0, y);
|
||||
CLIP_COORDS(x1, y);
|
||||
if(x0 > x1)
|
||||
{
|
||||
if (x0 > x1) {
|
||||
SWAP(x0, x1);
|
||||
}
|
||||
if(x0 == x1) return;
|
||||
if (x0 == x1) {
|
||||
return;
|
||||
}
|
||||
/* This is an optimised algorithm for writing horizontal lines.
|
||||
* We begin by finding the addresses of the x0 and x1 points. */
|
||||
int addr0 = CALC_BUFF_ADDR(x0, y);
|
||||
@ -407,22 +394,18 @@ void write_hline(uint8_t *buff, unsigned int x0, unsigned int x1, unsigned int y
|
||||
int mask, mask_l, mask_r, i;
|
||||
/* If the addresses are equal, we only need to write one word
|
||||
* which is an island. */
|
||||
if(addr0 == addr1)
|
||||
{
|
||||
if (addr0 == addr1) {
|
||||
mask = COMPUTE_HLINE_ISLAND_MASK(addr0_bit, addr1_bit);
|
||||
WRITE_WORD_MODE(buff, addr0, mask, mode);
|
||||
}
|
||||
} else {
|
||||
/* Otherwise we need to write the edges and then the middle. */
|
||||
else
|
||||
{
|
||||
mask_l = COMPUTE_HLINE_EDGE_L_MASK(addr0_bit);
|
||||
mask_r = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit);
|
||||
WRITE_WORD_MODE(buff, addr0, mask_l, mode);
|
||||
WRITE_WORD_MODE(buff, addr1, mask_r, mode);
|
||||
// Now write 0xffff words from start+1 to end-1.
|
||||
for(i = addr0 + 1; i <= addr1 - 1; i++)
|
||||
{
|
||||
uint8_t m=0xff;
|
||||
for (i = addr0 + 1; i <= addr1 - 1; i++) {
|
||||
uint8_t m = 0xff;
|
||||
WRITE_WORD_MODE(buff, i, m, mode);
|
||||
}
|
||||
}
|
||||
@ -461,8 +444,7 @@ void write_hline_outlined(unsigned int x0, unsigned int x1, unsigned int y, int
|
||||
{
|
||||
int stroke, fill;
|
||||
SETUP_STROKE_FILL(stroke, fill, mode)
|
||||
if(x0 > x1)
|
||||
{
|
||||
if (x0 > x1) {
|
||||
SWAP(x0, x1);
|
||||
}
|
||||
// Draw the main body of the line.
|
||||
@ -488,11 +470,12 @@ void write_vline(uint8_t *buff, unsigned int x, unsigned int y0, unsigned int y1
|
||||
unsigned int a;
|
||||
CLIP_COORDS(x, y0);
|
||||
CLIP_COORDS(x, y1);
|
||||
if(y0 > y1)
|
||||
{
|
||||
if (y0 > y1) {
|
||||
SWAP(y0, y1);
|
||||
}
|
||||
if(y0 == y1) return;
|
||||
if (y0 == y1) {
|
||||
return;
|
||||
}
|
||||
/* This is an optimised algorithm for writing vertical lines.
|
||||
* We begin by finding the addresses of the x,y0 and x,y1 points. */
|
||||
int addr0 = CALC_BUFF_ADDR(x, y0);
|
||||
@ -502,8 +485,7 @@ void write_vline(uint8_t *buff, unsigned int x, unsigned int y0, unsigned int y1
|
||||
uint16_t mask = 1 << (7 - bitnum);
|
||||
/* Run from addr0 to addr1 placing pixels. Increment by the number
|
||||
* of words n each graphics line. */
|
||||
for(a = addr0; a <= addr1; a += GRAPHICS_WIDTH_REAL / 8)
|
||||
{
|
||||
for (a = addr0; a <= addr1; a += GRAPHICS_WIDTH_REAL / 8) {
|
||||
WRITE_WORD_MODE(buff, a, mask, mode);
|
||||
}
|
||||
}
|
||||
@ -540,8 +522,7 @@ void write_vline_lm(unsigned int x, unsigned int y0, unsigned int y1, int lmode,
|
||||
void write_vline_outlined(unsigned int x, unsigned int y0, unsigned int y1, int endcap0, int endcap1, int mode, int mmode)
|
||||
{
|
||||
int stroke, fill;
|
||||
if(y0 > y1)
|
||||
{
|
||||
if (y0 > y1) {
|
||||
SWAP(y0, y1);
|
||||
}
|
||||
SETUP_STROKE_FILL(stroke, fill, mode);
|
||||
@ -574,7 +555,9 @@ void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsig
|
||||
CHECK_COORDS(x, y);
|
||||
CHECK_COORD_X(x + width);
|
||||
CHECK_COORD_Y(y + height);
|
||||
if(width <= 0 || height <= 0) return;
|
||||
if (width <= 0 || height <= 0) {
|
||||
return;
|
||||
}
|
||||
// Calculate as if the rectangle was only a horizontal line. We then
|
||||
// step these addresses through each row until we iterate `height` times.
|
||||
int addr0 = CALC_BUFF_ADDR(x, y);
|
||||
@ -583,26 +566,21 @@ void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsig
|
||||
int addr1_bit = CALC_BIT_IN_WORD(x + width);
|
||||
int mask, mask_l, mask_r, i;
|
||||
// If the addresses are equal, we need to write one word vertically.
|
||||
if(addr0 == addr1)
|
||||
{
|
||||
if (addr0 == addr1) {
|
||||
mask = COMPUTE_HLINE_ISLAND_MASK(addr0_bit, addr1_bit);
|
||||
while(height--)
|
||||
{
|
||||
while (height--) {
|
||||
WRITE_WORD_MODE(buff, addr0, mask, mode);
|
||||
addr0 += GRAPHICS_WIDTH_REAL / 8;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// Otherwise we need to write the edges and then the middle repeatedly.
|
||||
else
|
||||
{
|
||||
mask_l = COMPUTE_HLINE_EDGE_L_MASK(addr0_bit);
|
||||
mask_r = COMPUTE_HLINE_EDGE_R_MASK(addr1_bit);
|
||||
// Write edges first.
|
||||
yy = 0;
|
||||
addr0_old = addr0;
|
||||
addr1_old = addr1;
|
||||
while(yy < height)
|
||||
{
|
||||
while (yy < height) {
|
||||
WRITE_WORD_MODE(buff, addr0, mask_l, mode);
|
||||
WRITE_WORD_MODE(buff, addr1, mask_r, mode);
|
||||
addr0 += GRAPHICS_WIDTH_REAL / 8;
|
||||
@ -613,11 +591,9 @@ void write_filled_rectangle(uint8_t *buff, unsigned int x, unsigned int y, unsig
|
||||
yy = 0;
|
||||
addr0 = addr0_old;
|
||||
addr1 = addr1_old;
|
||||
while(yy < height)
|
||||
{
|
||||
for(i = addr0 + 1; i <= addr1 - 1; i++)
|
||||
{
|
||||
uint8_t m=0xff;
|
||||
while (yy < height) {
|
||||
for (i = addr0 + 1; i <= addr1 - 1; i++) {
|
||||
uint8_t m = 0xff;
|
||||
WRITE_WORD_MODE(buff, i, m, mode);
|
||||
}
|
||||
addr0 += GRAPHICS_WIDTH_REAL / 8;
|
||||
@ -681,16 +657,13 @@ void write_circle(uint8_t *buff, unsigned int cx, unsigned int cy, unsigned int
|
||||
{
|
||||
CHECK_COORDS(cx, cy);
|
||||
int error = -r, x = r, y = 0;
|
||||
while(x >= y)
|
||||
{
|
||||
if(dashp == 0 || (y % dashp) < (dashp / 2))
|
||||
{
|
||||
while (x >= y) {
|
||||
if (dashp == 0 || (y % dashp) < (dashp / 2)) {
|
||||
CIRCLE_PLOT_8(buff, cx, cy, x, y, mode);
|
||||
}
|
||||
error += (y * 2) + 1;
|
||||
y++;
|
||||
if(error >= 0)
|
||||
{
|
||||
if (error >= 0) {
|
||||
--x;
|
||||
error -= x * 2;
|
||||
}
|
||||
@ -716,10 +689,8 @@ void write_circle_outlined(unsigned int cx, unsigned int cy, unsigned int r, uns
|
||||
// This is a two step procedure. First, we draw the outline of the
|
||||
// circle, then we draw the inner part.
|
||||
int error = -r, x = r, y = 0;
|
||||
while(x >= y)
|
||||
{
|
||||
if(dashp == 0 || (y % dashp) < (dashp / 2))
|
||||
{
|
||||
while (x >= y) {
|
||||
if (dashp == 0 || (y % dashp) < (dashp / 2)) {
|
||||
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x + 1, y, mmode);
|
||||
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x + 1, y, stroke);
|
||||
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y + 1, mmode);
|
||||
@ -728,8 +699,7 @@ void write_circle_outlined(unsigned int cx, unsigned int cy, unsigned int r, uns
|
||||
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x - 1, y, stroke);
|
||||
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y - 1, mmode);
|
||||
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y - 1, stroke);
|
||||
if(bmode == 1)
|
||||
{
|
||||
if (bmode == 1) {
|
||||
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x + 1, y + 1, mmode);
|
||||
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x + 1, y + 1, stroke);
|
||||
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x - 1, y - 1, mmode);
|
||||
@ -738,8 +708,7 @@ void write_circle_outlined(unsigned int cx, unsigned int cy, unsigned int r, uns
|
||||
}
|
||||
error += (y * 2) + 1;
|
||||
y++;
|
||||
if(error >= 0)
|
||||
{
|
||||
if (error >= 0) {
|
||||
--x;
|
||||
error -= x * 2;
|
||||
}
|
||||
@ -747,17 +716,14 @@ void write_circle_outlined(unsigned int cx, unsigned int cy, unsigned int r, uns
|
||||
error = -r;
|
||||
x = r;
|
||||
y = 0;
|
||||
while(x >= y)
|
||||
{
|
||||
if(dashp == 0 || (y % dashp) < (dashp / 2))
|
||||
{
|
||||
while (x >= y) {
|
||||
if (dashp == 0 || (y % dashp) < (dashp / 2)) {
|
||||
CIRCLE_PLOT_8(draw_buffer_mask, cx, cy, x, y, mmode);
|
||||
CIRCLE_PLOT_8(draw_buffer_level, cx, cy, x, y, fill);
|
||||
}
|
||||
error += (y * 2) + 1;
|
||||
y++;
|
||||
if(error >= 0)
|
||||
{
|
||||
if (error >= 0) {
|
||||
--x;
|
||||
error -= x * 2;
|
||||
}
|
||||
@ -784,14 +750,11 @@ void write_circle_filled(uint8_t *buff, unsigned int cx, unsigned int cy, unsign
|
||||
//
|
||||
// Due to multiple writes to each set of pixels, we have a special exception
|
||||
// for when using the toggling draw mode.
|
||||
while(x >= y)
|
||||
{
|
||||
if(y != 0)
|
||||
{
|
||||
while (x >= y) {
|
||||
if (y != 0) {
|
||||
write_hline(buff, cx - x, cx + x, cy + y, mode);
|
||||
write_hline(buff, cx - x, cx + x, cy - y, mode);
|
||||
if(mode != 2 || (mode == 2 && xch && (cx - x) != (cx - y)))
|
||||
{
|
||||
if (mode != 2 || (mode == 2 && xch && (cx - x) != (cx - y))) {
|
||||
write_hline(buff, cx - y, cx + y, cy + x, mode);
|
||||
write_hline(buff, cx - y, cx + y, cy - x, mode);
|
||||
xch = 0;
|
||||
@ -799,16 +762,14 @@ void write_circle_filled(uint8_t *buff, unsigned int cx, unsigned int cy, unsign
|
||||
}
|
||||
error += (y * 2) + 1;
|
||||
y++;
|
||||
if(error >= 0)
|
||||
{
|
||||
if (error >= 0) {
|
||||
--x;
|
||||
xch = 1;
|
||||
error -= x * 2;
|
||||
}
|
||||
}
|
||||
// Handle toggle mode.
|
||||
if(mode == 2)
|
||||
{
|
||||
if (mode == 2) {
|
||||
write_hline(buff, cx - r, cx + r, cy, mode);
|
||||
}
|
||||
}
|
||||
@ -827,13 +788,11 @@ void write_line(uint8_t *buff, unsigned int x0, unsigned int y0, unsigned int x1
|
||||
{
|
||||
// Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
|
||||
int steep = abs(y1 - y0) > abs(x1 - x0);
|
||||
if(steep)
|
||||
{
|
||||
if (steep) {
|
||||
SWAP(x0, y0);
|
||||
SWAP(x1, y1);
|
||||
}
|
||||
if(x0 > x1)
|
||||
{
|
||||
if (x0 > x1) {
|
||||
SWAP(x0, x1);
|
||||
SWAP(y0, y1);
|
||||
}
|
||||
@ -843,23 +802,19 @@ void write_line(uint8_t *buff, unsigned int x0, unsigned int y0, unsigned int x1
|
||||
int ystep;
|
||||
int y = y0;
|
||||
int x; //, lasty = y, stox = 0;
|
||||
if(y0 < y1)
|
||||
if (y0 < y1) {
|
||||
ystep = 1;
|
||||
else
|
||||
} else {
|
||||
ystep = -1;
|
||||
for(x = x0; x < x1; x++)
|
||||
{
|
||||
if(steep)
|
||||
{
|
||||
write_pixel(buff, y, x, mode);
|
||||
}
|
||||
else
|
||||
{
|
||||
for (x = x0; x < x1; x++) {
|
||||
if (steep) {
|
||||
write_pixel(buff, y, x, mode);
|
||||
} else {
|
||||
write_pixel(buff, x, y, mode);
|
||||
}
|
||||
error -= deltay;
|
||||
if(error < 0)
|
||||
{
|
||||
if (error < 0) {
|
||||
y += ystep;
|
||||
error += deltax;
|
||||
}
|
||||
@ -900,24 +855,19 @@ void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsi
|
||||
// Based on http://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
|
||||
// This could be improved for speed.
|
||||
int omode, imode;
|
||||
if(mode == 0)
|
||||
{
|
||||
if (mode == 0) {
|
||||
omode = 0;
|
||||
imode = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
} else {
|
||||
omode = 1;
|
||||
imode = 0;
|
||||
}
|
||||
int steep = abs(y1 - y0) > abs(x1 - x0);
|
||||
if(steep)
|
||||
{
|
||||
if (steep) {
|
||||
SWAP(x0, y0);
|
||||
SWAP(x1, y1);
|
||||
}
|
||||
if(x0 > x1)
|
||||
{
|
||||
if (x0 > x1) {
|
||||
SWAP(x0, x1);
|
||||
SWAP(y0, y1);
|
||||
}
|
||||
@ -927,30 +877,26 @@ void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsi
|
||||
int ystep;
|
||||
int y = y0;
|
||||
int x;
|
||||
if(y0 < y1)
|
||||
if (y0 < y1) {
|
||||
ystep = 1;
|
||||
else
|
||||
} else {
|
||||
ystep = -1;
|
||||
}
|
||||
// Draw the outline.
|
||||
for(x = x0; x < x1; x++)
|
||||
{
|
||||
if(steep)
|
||||
{
|
||||
for (x = x0; x < x1; x++) {
|
||||
if (steep) {
|
||||
write_pixel_lm(y - 1, x, mmode, omode);
|
||||
write_pixel_lm(y + 1, x, mmode, omode);
|
||||
write_pixel_lm(y, x - 1, mmode, omode);
|
||||
write_pixel_lm(y, x + 1, mmode, omode);
|
||||
}
|
||||
else
|
||||
{
|
||||
} else {
|
||||
write_pixel_lm(x - 1, y, mmode, omode);
|
||||
write_pixel_lm(x + 1, y, mmode, omode);
|
||||
write_pixel_lm(x, y - 1, mmode, omode);
|
||||
write_pixel_lm(x, y + 1, mmode, omode);
|
||||
}
|
||||
error -= deltay;
|
||||
if(error < 0)
|
||||
{
|
||||
if (error < 0) {
|
||||
y += ystep;
|
||||
error += deltax;
|
||||
}
|
||||
@ -958,19 +904,14 @@ void write_line_outlined(unsigned int x0, unsigned int y0, unsigned int x1, unsi
|
||||
// Now draw the innards.
|
||||
error = deltax / 2;
|
||||
y = y0;
|
||||
for(x = x0; x < x1; x++)
|
||||
{
|
||||
if(steep)
|
||||
{
|
||||
for (x = x0; x < x1; x++) {
|
||||
if (steep) {
|
||||
write_pixel_lm(y, x, mmode, imode);
|
||||
}
|
||||
else
|
||||
{
|
||||
} else {
|
||||
write_pixel_lm(x, y, mmode, imode);
|
||||
}
|
||||
error -= deltay;
|
||||
if(error < 0)
|
||||
{
|
||||
if (error < 0) {
|
||||
y += ystep;
|
||||
error += deltax;
|
||||
}
|
||||
@ -995,8 +936,9 @@ void write_word_misaligned(uint8_t *buff, uint16_t word, unsigned int addr, unsi
|
||||
uint16_t lastmask = word << (16 - xoff);
|
||||
WRITE_WORD_MODE(buff, addr+1, firstmask && 0x00ff, mode);
|
||||
WRITE_WORD_MODE(buff, addr, (firstmask & 0xff00) >> 8, mode);
|
||||
if(xoff > 0)
|
||||
if (xoff > 0) {
|
||||
WRITE_WORD_MODE(buff, addr+2, (lastmask & 0xff00) >> 8, mode);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@ -1020,8 +962,9 @@ void write_word_misaligned_NAND(uint8_t *buff, uint16_t word, unsigned int addr,
|
||||
uint16_t lastmask = word << (16 - xoff);
|
||||
WRITE_WORD_NAND(buff, addr+1, firstmask & 0x00ff);
|
||||
WRITE_WORD_NAND(buff, addr, (firstmask & 0xff00) >> 8);
|
||||
if(xoff > 0)
|
||||
if (xoff > 0) {
|
||||
WRITE_WORD_NAND(buff, addr+2, (lastmask & 0xff00) >> 8);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@ -1045,8 +988,9 @@ void write_word_misaligned_OR(uint8_t *buff, uint16_t word, unsigned int addr, u
|
||||
uint16_t lastmask = word << (16 - xoff);
|
||||
WRITE_WORD_OR(buff, addr+1, firstmask & 0x00ff);
|
||||
WRITE_WORD_OR(buff, addr, (firstmask & 0xff00) >> 8);
|
||||
if(xoff > 0)
|
||||
if (xoff > 0) {
|
||||
WRITE_WORD_OR(buff, addr + 2, (lastmask & 0xff00) >> 8);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -1077,21 +1021,21 @@ void write_word_misaligned_lm(uint16_t wordl, uint16_t wordm, unsigned int addr,
|
||||
int fetch_font_info(uint8_t ch, int font, struct FontEntry *font_info, char *lookup)
|
||||
{
|
||||
// First locate the font struct.
|
||||
if(font > SIZEOF_ARRAY(fonts))
|
||||
return 0; // font does not exist, exit.*/
|
||||
if (font > SIZEOF_ARRAY(fonts)) {
|
||||
return 0; // font does not exist, exit.
|
||||
}
|
||||
// Load the font info; IDs are always sequential.
|
||||
*font_info = fonts[font];
|
||||
// Locate character in font lookup table. (If required.)
|
||||
if(lookup != NULL)
|
||||
{
|
||||
if (lookup != NULL) {
|
||||
*lookup = font_info->lookup[ch];
|
||||
if(*lookup == 0xff)
|
||||
if (*lookup == 0xff) {
|
||||
return 0; // character doesn't exist, don't bother writing it.
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* write_char16: Draw a character on the current draw buffer.
|
||||
* Currently supports outlined characters and characters with
|
||||
@ -1120,20 +1064,21 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font)
|
||||
// wide for now. Support for large characters may be added in future.
|
||||
{
|
||||
// Ensure we don't overflow.
|
||||
if(x + wbit > GRAPHICS_WIDTH_REAL)
|
||||
if (x + wbit > GRAPHICS_WIDTH_REAL) {
|
||||
return;
|
||||
}
|
||||
// Load data pointer.
|
||||
row = ch * font_info.height;
|
||||
row_temp = row;
|
||||
addr_temp = addr;
|
||||
xshift = 16 - font_info.width;
|
||||
// We can write mask words easily.
|
||||
for(yy = y; yy < y + font_info.height; yy++)
|
||||
{
|
||||
if(font==3)
|
||||
for (yy = y; yy < y + font_info.height; yy++) {
|
||||
if (font == 3) {
|
||||
write_word_misaligned_OR(draw_buffer_mask, font_mask12x18[row] << xshift, addr, wbit);
|
||||
else
|
||||
} else {
|
||||
write_word_misaligned_OR(draw_buffer_mask, font_mask8x10[row] << xshift, addr, wbit);
|
||||
}
|
||||
addr += GRAPHICS_WIDTH_REAL / 8;
|
||||
row++;
|
||||
}
|
||||
@ -1143,10 +1088,8 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font)
|
||||
// construct an AND mask and an OR mask, and apply each individually.
|
||||
row = row_temp;
|
||||
addr = addr_temp;
|
||||
for(yy = y; yy < y + font_info.height; yy++)
|
||||
{
|
||||
if(font==3)
|
||||
{
|
||||
for (yy = y; yy < y + font_info.height; yy++) {
|
||||
if (font == 3) {
|
||||
level_bits = font_frame12x18[row];
|
||||
//if(!(flags & FONT_INVERT)) // data is normally inverted
|
||||
level_bits = ~level_bits;
|
||||
@ -1169,8 +1112,6 @@ void write_char16(char ch, unsigned int x, unsigned int y, int font)
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* write_char: Draw a character on the current draw buffer.
|
||||
* Currently supports outlined characters and characters with
|
||||
@ -1201,19 +1142,18 @@ void write_char(char ch, unsigned int x, unsigned int y, int flags, int font)
|
||||
fetch_font_info(ch, font, &font_info, &lookup);
|
||||
// How big is the character? We handle characters up to 8 pixels
|
||||
// wide for now. Support for large characters may be added in future.
|
||||
if(font_info.width <= 8)
|
||||
{
|
||||
if (font_info.width <= 8) {
|
||||
// Ensure we don't overflow.
|
||||
if(x + wbit > GRAPHICS_WIDTH_REAL)
|
||||
if (x + wbit > GRAPHICS_WIDTH_REAL) {
|
||||
return;
|
||||
}
|
||||
// Load data pointer.
|
||||
row = lookup * font_info.height * 2;
|
||||
row_temp = row;
|
||||
addr_temp = addr;
|
||||
xshift = 16 - font_info.width;
|
||||
// We can write mask words easily.
|
||||
for(yy = y; yy < y + font_info.height; yy++)
|
||||
{
|
||||
for (yy = y; yy < y + font_info.height; yy++) {
|
||||
write_word_misaligned_OR(draw_buffer_mask, font_info.data[row] << xshift, addr, wbit);
|
||||
addr += GRAPHICS_WIDTH_REAL / 8;
|
||||
row++;
|
||||
@ -1224,11 +1164,12 @@ void write_char(char ch, unsigned int x, unsigned int y, int flags, int font)
|
||||
// construct an AND mask and an OR mask, and apply each individually.
|
||||
row = row_temp;
|
||||
addr = addr_temp;
|
||||
for(yy = y; yy < y + font_info.height; yy++)
|
||||
{
|
||||
for (yy = y; yy < y + font_info.height; yy++) {
|
||||
level_bits = font_info.data[row + font_info.height];
|
||||
if(!(flags & FONT_INVERT)) // data is normally inverted
|
||||
if (!(flags & FONT_INVERT)) {
|
||||
// data is normally inverted
|
||||
level_bits = ~level_bits;
|
||||
}
|
||||
or_mask = font_info.data[row] << xshift;
|
||||
and_mask = (font_info.data[row] & level_bits) << xshift;
|
||||
write_word_misaligned_OR(draw_buffer_level, or_mask, addr, wbit);
|
||||
@ -1242,51 +1183,51 @@ void write_char(char ch, unsigned int x, unsigned int y, int flags, int font)
|
||||
}
|
||||
|
||||
/**
|
||||
* calc_text_dimensions: Calculate the dimensions of a
|
||||
* string in a given font. Supports new lines and
|
||||
* carriage returns in text.
|
||||
*
|
||||
* @param str string to calculate dimensions of
|
||||
* @param font_info font info structure
|
||||
* @param xs horizontal spacing
|
||||
* @param ys vertical spacing
|
||||
* @param dim return result: struct FontDimensions
|
||||
*/
|
||||
* calc_text_dimensions: Calculate the dimensions of a
|
||||
* string in a given font. Supports new lines and
|
||||
* carriage returns in text.
|
||||
*
|
||||
* @param str string to calculate dimensions of
|
||||
* @param font_info font info structure
|
||||
* @param xs horizontal spacing
|
||||
* @param ys vertical spacing
|
||||
* @param dim return result: struct FontDimensions
|
||||
*/
|
||||
void calc_text_dimensions(char *str, struct FontEntry font, int xs, int ys, struct FontDimensions *dim)
|
||||
{
|
||||
int max_length = 0, line_length = 0, lines = 1;
|
||||
while(*str != 0)
|
||||
{
|
||||
while (*str != 0) {
|
||||
line_length++;
|
||||
if(*str == '\n' || *str == '\r')
|
||||
{
|
||||
if(line_length > max_length)
|
||||
if (*str == '\n' || *str == '\r') {
|
||||
if (line_length > max_length) {
|
||||
max_length = line_length;
|
||||
}
|
||||
line_length = 0;
|
||||
lines++;
|
||||
}
|
||||
str++;
|
||||
}
|
||||
if(line_length > max_length)
|
||||
if (line_length > max_length) {
|
||||
max_length = line_length;
|
||||
}
|
||||
dim->width = max_length * (font.width + xs);
|
||||
dim->height = lines * (font.height + ys);
|
||||
}
|
||||
|
||||
/**
|
||||
* write_string: Draw a string on the screen with certain
|
||||
* alignment parameters.
|
||||
*
|
||||
* @param str string to write
|
||||
* @param x x coordinate
|
||||
* @param y y coordinate
|
||||
* @param xs horizontal spacing
|
||||
* @param ys horizontal spacing
|
||||
* @param va vertical align
|
||||
* @param ha horizontal align
|
||||
* @param flags flags (passed to write_char)
|
||||
* @param font font
|
||||
*/
|
||||
* write_string: Draw a string on the screen with certain
|
||||
* alignment parameters.
|
||||
*
|
||||
* @param str string to write
|
||||
* @param x x coordinate
|
||||
* @param y y coordinate
|
||||
* @param xs horizontal spacing
|
||||
* @param ys horizontal spacing
|
||||
* @param va vertical align
|
||||
* @param ha horizontal align
|
||||
* @param flags flags (passed to write_char)
|
||||
* @param font font
|
||||
*/
|
||||
void write_string(char *str, unsigned int x, unsigned int y, unsigned int xs, unsigned int ys, int va, int ha, int flags, int font)
|
||||
{
|
||||
int xx = 0, yy = 0, xx_original = 0;
|
||||
@ -1295,36 +1236,42 @@ void write_string(char *str, unsigned int x, unsigned int y, unsigned int xs, un
|
||||
// Determine font info and dimensions/position of the string.
|
||||
fetch_font_info(0, font, &font_info, NULL);
|
||||
calc_text_dimensions(str, font_info, xs, ys, &dim);
|
||||
switch(va)
|
||||
{
|
||||
case TEXT_VA_TOP: yy = y; break;
|
||||
case TEXT_VA_MIDDLE: yy = y - (dim.height / 2); break;
|
||||
case TEXT_VA_BOTTOM: yy = y - dim.height; break;
|
||||
switch (va) {
|
||||
case TEXT_VA_TOP:
|
||||
yy = y;
|
||||
break;
|
||||
case TEXT_VA_MIDDLE:
|
||||
yy = y - (dim.height / 2);
|
||||
break;
|
||||
case TEXT_VA_BOTTOM:
|
||||
yy = y - dim.height;
|
||||
break;
|
||||
}
|
||||
switch(ha)
|
||||
{
|
||||
case TEXT_HA_LEFT: xx = x; break;
|
||||
case TEXT_HA_CENTER: xx = x - (dim.width / 2); break;
|
||||
case TEXT_HA_RIGHT: xx = x - dim.width; break;
|
||||
switch (ha) {
|
||||
case TEXT_HA_LEFT:
|
||||
xx = x;
|
||||
break;
|
||||
case TEXT_HA_CENTER:
|
||||
xx = x - (dim.width / 2);
|
||||
break;
|
||||
case TEXT_HA_RIGHT:
|
||||
xx = x - dim.width;
|
||||
break;
|
||||
}
|
||||
// Then write each character.
|
||||
xx_original = xx;
|
||||
while(*str != 0)
|
||||
{
|
||||
if(*str == '\n' || *str == '\r')
|
||||
{
|
||||
while (*str != 0) {
|
||||
if (*str == '\n' || *str == '\r') {
|
||||
yy += ys + font_info.height;
|
||||
xx = xx_original;
|
||||
}
|
||||
else
|
||||
{
|
||||
if(xx >= 0 && xx < GRAPHICS_WIDTH_REAL)
|
||||
{
|
||||
if(font_info.id<2)
|
||||
} else {
|
||||
if (xx >= 0 && xx < GRAPHICS_WIDTH_REAL) {
|
||||
if (font_info.id < 2) {
|
||||
write_char(*str, xx, yy, flags, font);
|
||||
else
|
||||
} else {
|
||||
write_char16(*str, xx, yy, font);
|
||||
}
|
||||
}
|
||||
xx += font_info.width + xs;
|
||||
}
|
||||
str++;
|
||||
@ -1332,18 +1279,18 @@ void write_string(char *str, unsigned int x, unsigned int y, unsigned int xs, un
|
||||
}
|
||||
|
||||
/**
|
||||
* write_string_formatted: Draw a string with format escape
|
||||
* sequences in it. Allows for complex text effects.
|
||||
*
|
||||
* @param str string to write (with format data)
|
||||
* @param x x coordinate
|
||||
* @param y y coordinate
|
||||
* @param xs default horizontal spacing
|
||||
* @param ys default horizontal spacing
|
||||
* @param va vertical align
|
||||
* @param ha horizontal align
|
||||
* @param flags flags (passed to write_char)
|
||||
*/
|
||||
* write_string_formatted: Draw a string with format escape
|
||||
* sequences in it. Allows for complex text effects.
|
||||
*
|
||||
* @param str string to write (with format data)
|
||||
* @param x x coordinate
|
||||
* @param y y coordinate
|
||||
* @param xs default horizontal spacing
|
||||
* @param ys default horizontal spacing
|
||||
* @param va vertical align
|
||||
* @param ha horizontal align
|
||||
* @param flags flags (passed to write_char)
|
||||
*/
|
||||
void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned int xs, unsigned int ys, int va, int ha, int flags)
|
||||
{
|
||||
int fcode = 0, fptr = 0, font = 0, fwidth = 0, fheight = 0, xx = x, yy = y, max_xx = 0, max_height = 0;
|
||||
@ -1360,53 +1307,51 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned
|
||||
// work out a bounding box. We'll parse again for the final output.
|
||||
// This is a simple state machine parser.
|
||||
char *ostr = str;
|
||||
while(*str)
|
||||
{
|
||||
if(*str == '<' && fcode == 1) // escape code: skip
|
||||
while (*str) {
|
||||
if (*str == '<' && fcode == 1) {
|
||||
// escape code: skip
|
||||
fcode = 0;
|
||||
if(*str == '<' && fcode == 0) // begin format code?
|
||||
{
|
||||
}
|
||||
if (*str == '<' && fcode == 0) {
|
||||
// begin format code?
|
||||
fcode = 1;
|
||||
fptr = 0;
|
||||
}
|
||||
if(*str == '>' && fcode == 1)
|
||||
{
|
||||
if (*str == '>' && fcode == 1) {
|
||||
fcode = 0;
|
||||
if(strcmp(fstack, "B")) // switch to "big" font (font #1)
|
||||
{
|
||||
if (strcmp(fstack, "B")) {
|
||||
// switch to "big" font (font #1)
|
||||
fwidth = bigfontwidth;
|
||||
fheight = bigfontheight;
|
||||
}
|
||||
else if(strcmp(fstack, "S")) // switch to "small" font (font #0)
|
||||
{
|
||||
} else if (strcmp(fstack, "S")) {
|
||||
// switch to "small" font (font #0)
|
||||
fwidth = smallfontwidth;
|
||||
fheight = smallfontheight;
|
||||
}
|
||||
if(fheight > max_height)
|
||||
if (fheight > max_height) {
|
||||
max_height = fheight;
|
||||
}
|
||||
// Skip over this byte. Go to next byte.
|
||||
str++;
|
||||
continue;
|
||||
}
|
||||
if(*str != '<' && *str != '>' && fcode == 1)
|
||||
{
|
||||
if (*str != '<' && *str != '>' && fcode == 1) {
|
||||
// Add to the format stack (up to 10 bytes.)
|
||||
if(fptr > 10) // stop adding bytes
|
||||
{
|
||||
if (fptr > 10) {
|
||||
// stop adding bytes
|
||||
str++; // go to next byte
|
||||
continue;
|
||||
}
|
||||
fstack[fptr++] = *str;
|
||||
fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.)
|
||||
}
|
||||
if(fcode == 0)
|
||||
{
|
||||
if (fcode == 0) {
|
||||
// Not a format code, raw text.
|
||||
xx += fwidth + xs;
|
||||
if(*str == '\n')
|
||||
{
|
||||
if(xx > max_xx)
|
||||
if (*str == '\n') {
|
||||
if (xx > max_xx) {
|
||||
max_xx = xx;
|
||||
}
|
||||
xx = x;
|
||||
yy += fheight + ys;
|
||||
}
|
||||
@ -1439,26 +1384,25 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned
|
||||
font = 0;
|
||||
xx = 0;
|
||||
yy = 0;
|
||||
while(*str)
|
||||
{
|
||||
if(*str == '<' && fcode == 1) // escape code: skip
|
||||
while (*str) {
|
||||
if (*str == '<' && fcode == 1) {
|
||||
// escape code: skip
|
||||
fcode = 0;
|
||||
if(*str == '<' && fcode == 0) // begin format code?
|
||||
{
|
||||
}
|
||||
if (*str == '<' && fcode == 0) {
|
||||
// begin format code?
|
||||
fcode = 1;
|
||||
fptr = 0;
|
||||
}
|
||||
if(*str == '>' && fcode == 1)
|
||||
{
|
||||
if (*str == '>' && fcode == 1) {
|
||||
fcode = 0;
|
||||
if(strcmp(fstack, "B")) // switch to "big" font (font #1)
|
||||
{
|
||||
if (strcmp(fstack, "B")) {
|
||||
// switch to "big" font (font #1)
|
||||
fwidth = bigfontwidth;
|
||||
fheight = bigfontheight;
|
||||
font = 1;
|
||||
}
|
||||
else if(strcmp(fstack, "S")) // switch to "small" font (font #0)
|
||||
{
|
||||
} else if (strcmp(fstack, "S")) {
|
||||
// switch to "small" font (font #0)
|
||||
fwidth = smallfontwidth;
|
||||
fheight = smallfontheight;
|
||||
font = 0;
|
||||
@ -1467,27 +1411,25 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned
|
||||
str++;
|
||||
continue;
|
||||
}
|
||||
if(*str != '<' && *str != '>' && fcode == 1)
|
||||
{
|
||||
if (*str != '<' && *str != '>' && fcode == 1) {
|
||||
// Add to the format stack (up to 10 bytes.)
|
||||
if(fptr > 10) // stop adding bytes
|
||||
{
|
||||
if (fptr > 10) {
|
||||
// stop adding bytes
|
||||
str++; // go to next byte
|
||||
continue;
|
||||
}
|
||||
fstack[fptr++] = *str;
|
||||
fstack[fptr] = '\0'; // clear next byte (ready for next char or to terminate string.)
|
||||
}
|
||||
if(fcode == 0)
|
||||
{
|
||||
if (fcode == 0) {
|
||||
// Not a format code, raw text. So we draw it.
|
||||
// TODO - different font sizes.
|
||||
write_char(*str, xx, yy + (max_height - fheight), flags, font);
|
||||
xx += fwidth + xs;
|
||||
if(*str == '\n')
|
||||
{
|
||||
if(xx > max_xx)
|
||||
if (*str == '\n') {
|
||||
if (xx > max_xx) {
|
||||
max_xx = xx;
|
||||
}
|
||||
xx = x;
|
||||
yy += fheight + ys;
|
||||
}
|
||||
@ -1496,86 +1438,81 @@ void write_string_formatted(char *str, unsigned int x, unsigned int y, unsigned
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//SUPEROSD-
|
||||
|
||||
|
||||
// graphics
|
||||
|
||||
void drawAttitude(uint16_t x, uint16_t y, int16_t pitch, int16_t roll, uint16_t size)
|
||||
{
|
||||
int16_t a = mySin(roll+360);
|
||||
int16_t b = myCos(roll+360);
|
||||
int16_t c = mySin(roll+90+360)*5/100;
|
||||
int16_t d = myCos(roll+90+360)*5/100;
|
||||
int16_t a = mySin(roll + 360);
|
||||
int16_t b = myCos(roll + 360);
|
||||
int16_t c = mySin(roll + 90 + 360) * 5 / 100;
|
||||
int16_t d = myCos(roll + 90 + 360) * 5 / 100;
|
||||
|
||||
int16_t k;
|
||||
int16_t l;
|
||||
|
||||
int16_t indi30x1=myCos(30)*(size/2+1) / 100;
|
||||
int16_t indi30y1=mySin(30)*(size/2+1) / 100;
|
||||
int16_t indi30x1 = myCos(30) * (size / 2 + 1) / 100;
|
||||
int16_t indi30y1 = mySin(30) * (size / 2 + 1) / 100;
|
||||
|
||||
int16_t indi30x2=myCos(30)*(size/2+4) / 100;
|
||||
int16_t indi30y2=mySin(30)*(size/2+4) / 100;
|
||||
int16_t indi30x2 = myCos(30) * (size / 2 + 4) / 100;
|
||||
int16_t indi30y2 = mySin(30) * (size / 2 + 4) / 100;
|
||||
|
||||
int16_t indi60x1=myCos(60)*(size/2+1) / 100;
|
||||
int16_t indi60y1=mySin(60)*(size/2+1) / 100;
|
||||
int16_t indi60x1 = myCos(60) * (size / 2 + 1) / 100;
|
||||
int16_t indi60y1 = mySin(60) * (size / 2 + 1) / 100;
|
||||
|
||||
int16_t indi60x2=myCos(60)*(size/2+4) / 100;
|
||||
int16_t indi60y2=mySin(60)*(size/2+4) / 100;
|
||||
int16_t indi60x2 = myCos(60) * (size / 2 + 4) / 100;
|
||||
int16_t indi60y2 = mySin(60) * (size / 2 + 4) / 100;
|
||||
|
||||
pitch=pitch%90;
|
||||
if(pitch>90)
|
||||
{
|
||||
pitch=pitch-90;
|
||||
pitch = pitch % 90;
|
||||
if (pitch > 90) {
|
||||
pitch = pitch - 90;
|
||||
}
|
||||
if(pitch<-90)
|
||||
{
|
||||
pitch=pitch+90;
|
||||
if (pitch < -90) {
|
||||
pitch = pitch + 90;
|
||||
}
|
||||
a = (a * (size/2)) / 100;
|
||||
b = (b * (size/2)) / 100;
|
||||
a = (a * (size / 2)) / 100;
|
||||
b = (b * (size / 2)) / 100;
|
||||
|
||||
if(roll<-90 || roll>90)
|
||||
pitch=pitch*-1;
|
||||
k = a*pitch/90;
|
||||
l = b*pitch/90;
|
||||
if (roll < -90 || roll > 90) {
|
||||
pitch = pitch * -1;
|
||||
}
|
||||
k = a * pitch / 90;
|
||||
l = b * pitch / 90;
|
||||
|
||||
// scale
|
||||
//0
|
||||
//drawLine((x)-1-(size/2+4), (y)-1, (x)-1 - (size/2+1), (y)-1);
|
||||
//drawLine((x)-1+(size/2+4), (y)-1, (x)-1 + (size/2+1), (y)-1);
|
||||
write_line_outlined((x)-1-(size/2+4), (y)-1, (x)-1 - (size/2+1), (y)-1,0,0,0,1);
|
||||
write_line_outlined((x)-1+(size/2+4), (y)-1, (x)-1 + (size/2+1), (y)-1,0,0,0,1);
|
||||
write_line_outlined((x) - 1 - (size / 2 + 4), (y) - 1, (x) - 1 - (size / 2 + 1), (y) - 1, 0, 0, 0, 1);
|
||||
write_line_outlined((x) - 1 + (size / 2 + 4), (y) - 1, (x) - 1 + (size / 2 + 1), (y) - 1, 0, 0, 0, 1);
|
||||
|
||||
//30
|
||||
//drawLine((x)-1+indi30x1, (y)-1-indi30y1, (x)-1 + indi30x2, (y)-1 - indi30y2);
|
||||
//drawLine((x)-1-indi30x1, (y)-1-indi30y1, (x)-1 - indi30x2, (y)-1 - indi30y2);
|
||||
write_line_outlined((x)-1+indi30x1, (y)-1-indi30y1, (x)-1 + indi30x2, (y)-1 - indi30y2,0,0,0,1);
|
||||
write_line_outlined((x)-1-indi30x1, (y)-1-indi30y1, (x)-1 - indi30x2, (y)-1 - indi30y2,0,0,0,1);
|
||||
write_line_outlined((x) - 1 + indi30x1, (y) - 1 - indi30y1, (x) - 1 + indi30x2, (y) - 1 - indi30y2, 0, 0, 0, 1);
|
||||
write_line_outlined((x) - 1 - indi30x1, (y) - 1 - indi30y1, (x) - 1 - indi30x2, (y) - 1 - indi30y2, 0, 0, 0, 1);
|
||||
//60
|
||||
//drawLine((x)-1+indi60x1, (y)-1-indi60y1, (x)-1 + indi60x2, (y)-1 - indi60y2);
|
||||
//drawLine((x)-1-indi60x1, (y)-1-indi60y1, (x)-1 - indi60x2, (y)-1 - indi60y2);
|
||||
write_line_outlined((x)-1+indi60x1, (y)-1-indi60y1, (x)-1 + indi60x2, (y)-1 - indi60y2,0,0,0,1);
|
||||
write_line_outlined((x)-1-indi60x1, (y)-1-indi60y1, (x)-1 - indi60x2, (y)-1 - indi60y2,0,0,0,1);
|
||||
write_line_outlined((x) - 1 + indi60x1, (y) - 1 - indi60y1, (x) - 1 + indi60x2, (y) - 1 - indi60y2, 0, 0, 0, 1);
|
||||
write_line_outlined((x) - 1 - indi60x1, (y) - 1 - indi60y1, (x) - 1 - indi60x2, (y) - 1 - indi60y2, 0, 0, 0, 1);
|
||||
//90
|
||||
//drawLine((x)-1, (y)-1-(size/2+4), (x)-1, (y)-1 - (size/2+1));
|
||||
write_line_outlined((x)-1, (y)-1-(size/2+4), (x)-1, (y)-1 - (size/2+1),0,0,0,1);
|
||||
|
||||
write_line_outlined((x) - 1, (y) - 1 - (size / 2 + 4), (x) - 1, (y) - 1 - (size / 2 + 1), 0, 0, 0, 1);
|
||||
|
||||
//roll
|
||||
//drawLine((x)-1 - b, (y)-1 + a, (x)-1 + b, (y)-1 - a); //Direction line
|
||||
write_line_outlined((x)-1 - b, (y)-1 + a, (x)-1 + b, (y)-1 - a,0,0,0,1); //Direction line
|
||||
write_line_outlined((x) - 1 - b, (y) - 1 + a, (x) - 1 + b, (y) - 1 - a, 0, 0, 0, 1); //Direction line
|
||||
//"wingtips"
|
||||
//drawLine((x)-1 - b, (y)-1 + a, (x)-1 - b + d, (y)-1 + a - c);
|
||||
//drawLine((x)-1 + b + d, (y)-1 - a - c, (x)-1 + b, (y)-1 - a);
|
||||
write_line_outlined((x)-1 - b, (y)-1 + a, (x)-1 - b + d, (y)-1 + a - c,0,0,0,1);
|
||||
write_line_outlined((x)-1 + b + d, (y)-1 - a - c, (x)-1 + b, (y)-1 - a,0,0,0,1);
|
||||
write_line_outlined((x) - 1 - b, (y) - 1 + a, (x) - 1 - b + d, (y) - 1 + a - c, 0, 0, 0, 1);
|
||||
write_line_outlined((x) - 1 + b + d, (y) - 1 - a - c, (x) - 1 + b, (y) - 1 - a, 0, 0, 0, 1);
|
||||
|
||||
//pitch
|
||||
//drawLine((x)-1, (y)-1, (x)-1 - k, (y)-1 - l);
|
||||
write_line_outlined((x)-1, (y)-1, (x)-1 - k, (y)-1 - l,0,0,0,1);
|
||||
|
||||
write_line_outlined((x) - 1, (y) - 1, (x) - 1 - k, (y) - 1 - l, 0, 0, 0, 1);
|
||||
|
||||
//drawCircle(x-1, y-1, 5);
|
||||
//write_circle_outlined(x-1, y-1, 5,0,0,0,1);
|
||||
@ -1585,7 +1522,7 @@ void drawAttitude(uint16_t x, uint16_t y, int16_t pitch, int16_t roll, uint16_t
|
||||
|
||||
void drawBattery(uint16_t x, uint16_t y, uint8_t battery, uint16_t size)
|
||||
{
|
||||
int i=0;
|
||||
int i = 0;
|
||||
int batteryLines;
|
||||
//top
|
||||
/*drawLine((x)-1+(size/2-size/4), (y)-1, (x)-1 + (size/2+size/4), (y)-1);
|
||||
@ -1600,30 +1537,30 @@ void drawBattery(uint16_t x, uint16_t y, uint8_t battery, uint16_t size)
|
||||
//right
|
||||
drawLine((x)-1+size, (y)-1+2, (x)-1+size, (y)-1+size*3);*/
|
||||
|
||||
write_rectangle_outlined((x)-1, (y)-1+2,size,size*3,0,1);
|
||||
write_vline_lm((x)-1+(size/2+size/4)+1,(y)-2,(y)-1+1,0,1);
|
||||
write_vline_lm((x)-1+(size/2-size/4)-1,(y)-2,(y)-1+1,0,1);
|
||||
write_hline_lm((x)-1+(size/2-size/4),(x)-1 + (size/2+size/4),(y)-2,0,1);
|
||||
write_hline_lm((x)-1+(size/2-size/4),(x)-1 + (size/2+size/4),(y)-1,1,1);
|
||||
write_hline_lm((x)-1+(size/2-size/4),(x)-1 + (size/2+size/4),(y)-1+1,1,1);
|
||||
write_rectangle_outlined((x) - 1, (y) - 1 + 2, size, size * 3, 0, 1);
|
||||
write_vline_lm((x) - 1 + (size / 2 + size / 4) + 1, (y) - 2, (y) - 1 + 1, 0, 1);
|
||||
write_vline_lm((x) - 1 + (size / 2 - size / 4) - 1, (y) - 2, (y) - 1 + 1, 0, 1);
|
||||
write_hline_lm((x) - 1 + (size / 2 - size / 4), (x) - 1 + (size / 2 + size / 4), (y) - 2, 0, 1);
|
||||
write_hline_lm((x) - 1 + (size / 2 - size / 4), (x) - 1 + (size / 2 + size / 4), (y) - 1, 1, 1);
|
||||
write_hline_lm((x) - 1 + (size / 2 - size / 4), (x) - 1 + (size / 2 + size / 4), (y) - 1 + 1, 1, 1);
|
||||
|
||||
batteryLines = battery*(size*3-2)/100;
|
||||
for(i=0;i<batteryLines;i++)
|
||||
{
|
||||
write_hline_lm((x)-1,(x)-1 + size,(y)-1+size*3-i,1,1);
|
||||
batteryLines = battery * (size * 3 - 2) / 100;
|
||||
for (i = 0; i < batteryLines; i++) {
|
||||
write_hline_lm((x) - 1, (x) - 1 + size, (y) - 1 + size * 3 - i, 1, 1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void printTime(uint16_t x, uint16_t y) {
|
||||
char temp[9]={0};
|
||||
sprintf(temp,"%02d:%02d:%02d",timex.hour,timex.min,timex.sec);
|
||||
void printTime(uint16_t x, uint16_t y)
|
||||
{
|
||||
char temp[9] =
|
||||
{ 0 };
|
||||
sprintf(temp, "%02d:%02d:%02d", timex.hour, timex.min, timex.sec);
|
||||
//printTextFB(x,y,temp);
|
||||
write_string(temp, x, y, 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3);
|
||||
}
|
||||
|
||||
/*
|
||||
void drawAltitude(uint16_t x, uint16_t y, int16_t alt, uint8_t dir) {
|
||||
void drawAltitude(uint16_t x, uint16_t y, int16_t alt, uint8_t dir) {
|
||||
|
||||
char temp[9]={0};
|
||||
char updown=' ';
|
||||
@ -1636,7 +1573,7 @@ void drawAltitude(uint16_t x, uint16_t y, int16_t alt, uint8_t dir) {
|
||||
printTextFB(charx,y+2,temp);
|
||||
// frame
|
||||
drawBox(charx*16-3,y,charx*16+strlen(temp)*8+3,y+11);
|
||||
}*/
|
||||
}*/
|
||||
|
||||
/**
|
||||
* hud_draw_vertical_scale: Draw a vertical scale.
|
||||
@ -1655,27 +1592,25 @@ void drawAltitude(uint16_t x, uint16_t y, int16_t alt, uint8_t dir) {
|
||||
* @param max_val maximum expected value (used to compute size of arrow ticker)
|
||||
* @param flags special flags (see hud.h.)
|
||||
*/
|
||||
void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int height, int mintick_step, int majtick_step, int mintick_len, int majtick_len, int boundtick_len, int max_val, int flags)
|
||||
void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int height, int mintick_step, int majtick_step, int mintick_len, int majtick_len,
|
||||
int boundtick_len, int max_val, int flags)
|
||||
{
|
||||
char temp[15];//, temp2[15];
|
||||
char temp[15]; //, temp2[15];
|
||||
struct FontEntry font_info;
|
||||
struct FontDimensions dim;
|
||||
// Halign should be in a small span.
|
||||
//MY_ASSERT(halign >= -1 && halign <= 1);
|
||||
// Compute the position of the elements.
|
||||
int majtick_start = 0, majtick_end = 0, mintick_start = 0, mintick_end = 0, boundtick_start = 0, boundtick_end = 0;
|
||||
if(halign == -1)
|
||||
{
|
||||
if (halign == -1) {
|
||||
majtick_start = x;
|
||||
majtick_end = x + majtick_len;
|
||||
mintick_start = x;
|
||||
mintick_end = x + mintick_len;
|
||||
boundtick_start = x;
|
||||
boundtick_end = x + boundtick_len;
|
||||
}
|
||||
else if(halign == +1)
|
||||
{
|
||||
x=x-GRAPHICS_HDEADBAND;
|
||||
} else if (halign == +1) {
|
||||
x = x - GRAPHICS_HDEADBAND;
|
||||
majtick_start = GRAPHICS_WIDTH_REAL - x - 1;
|
||||
majtick_end = GRAPHICS_WIDTH_REAL - x - majtick_len - 1;
|
||||
mintick_start = GRAPHICS_WIDTH_REAL - x - 1;
|
||||
@ -1693,69 +1628,61 @@ void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int hei
|
||||
int range_2 = range / 2; //, height_2 = height / 2;
|
||||
int r = 0, rr = 0, rv = 0, ys = 0, style = 0; //calc_ys = 0,
|
||||
// Iterate through each step.
|
||||
for(r = -range_2; r <= +range_2; r++)
|
||||
{
|
||||
for (r = -range_2; r <= +range_2; r++) {
|
||||
style = 0;
|
||||
rr = r + range_2 - v; // normalise range for modulo, subtract value to move ticker tape
|
||||
rv = -rr + range_2; // for number display
|
||||
if(flags & HUD_VSCALE_FLAG_NO_NEGATIVE)
|
||||
if (flags & HUD_VSCALE_FLAG_NO_NEGATIVE)
|
||||
rr += majtick_step / 2;
|
||||
if(rr % majtick_step == 0)
|
||||
if (rr % majtick_step == 0)
|
||||
style = 1; // major tick
|
||||
else if(rr % mintick_step == 0)
|
||||
else if (rr % mintick_step == 0)
|
||||
style = 2; // minor tick
|
||||
else
|
||||
style = 0;
|
||||
if(flags & HUD_VSCALE_FLAG_NO_NEGATIVE && rv < 0)
|
||||
if (flags & HUD_VSCALE_FLAG_NO_NEGATIVE && rv < 0)
|
||||
continue;
|
||||
if(style)
|
||||
{
|
||||
if (style) {
|
||||
// Calculate y position.
|
||||
ys = ((long int)(r * height) / (long int)range) + y;
|
||||
ys = ((long int) (r * height) / (long int) range) + y;
|
||||
//sprintf(temp, "ys=%d", ys);
|
||||
//con_puts(temp, 0);
|
||||
// Depending on style, draw a minor or a major tick.
|
||||
if(style == 1)
|
||||
{
|
||||
if (style == 1) {
|
||||
write_hline_outlined(majtick_start, majtick_end, ys, 2, 2, 0, 1);
|
||||
memset(temp, ' ', 10);
|
||||
//my_itoa(rv, temp);
|
||||
sprintf(temp,"%d",rv);
|
||||
sprintf(temp, "%d", rv);
|
||||
text_length = (strlen(temp) + 1) * small_font_char_width; // add 1 for margin
|
||||
if(text_length > max_text_y)
|
||||
if (text_length > max_text_y)
|
||||
max_text_y = text_length;
|
||||
if(halign == -1)
|
||||
if (halign == -1)
|
||||
write_string(temp, majtick_end + text_x_spacing, ys, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_LEFT, 0, 1);
|
||||
else
|
||||
write_string(temp, majtick_end - text_x_spacing + 1, ys, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_RIGHT, 0, 1);
|
||||
}
|
||||
else if(style == 2)
|
||||
} else if (style == 2)
|
||||
write_hline_outlined(mintick_start, mintick_end, ys, 2, 2, 0, 1);
|
||||
}
|
||||
}
|
||||
// Generate the string for the value, as well as calculating its dimensions.
|
||||
memset(temp, ' ', 10);
|
||||
//my_itoa(v, temp);
|
||||
sprintf(temp,"%d",v);
|
||||
sprintf(temp, "%d", v);
|
||||
// TODO: add auto-sizing.
|
||||
calc_text_dimensions(temp, font_info, 1, 0, &dim);
|
||||
int xx = 0, i = 0;
|
||||
if(halign == -1)
|
||||
if (halign == -1)
|
||||
xx = majtick_end + text_x_spacing;
|
||||
else
|
||||
xx = majtick_end - text_x_spacing;
|
||||
// Draw an arrow from the number to the point.
|
||||
for(i = 0; i < arrow_len; i++)
|
||||
{
|
||||
if(halign == -1)
|
||||
{
|
||||
for (i = 0; i < arrow_len; i++) {
|
||||
if (halign == -1) {
|
||||
write_pixel_lm(xx - arrow_len + i, y - i - 1, 1, 1);
|
||||
write_pixel_lm(xx - arrow_len + i, y + i - 1, 1, 1);
|
||||
write_hline_lm(xx + dim.width - 1, xx - arrow_len + i + 1, y - i - 1, 0, 1);
|
||||
write_hline_lm(xx + dim.width - 1, xx - arrow_len + i + 1, y + i - 1, 0, 1);
|
||||
}
|
||||
else
|
||||
{
|
||||
} else {
|
||||
write_pixel_lm(xx + arrow_len - i, y - i - 1, 1, 1);
|
||||
write_pixel_lm(xx + arrow_len - i, y + i - 1, 1, 1);
|
||||
write_hline_lm(xx - dim.width - 1, xx + arrow_len - i - 1, y - i - 1, 0, 1);
|
||||
@ -1765,33 +1692,29 @@ void hud_draw_vertical_scale(int v, int range, int halign, int x, int y, int hei
|
||||
// write_hline_lm(xx - dim.width - 1, xx + (arrow_len - i), y - i - 1, 1, 1);
|
||||
// write_hline_lm(xx - dim.width - 1, xx + (arrow_len - i), y + i - 1, 1, 1);
|
||||
}
|
||||
if(halign == -1)
|
||||
{
|
||||
if (halign == -1) {
|
||||
write_hline_lm(xx, xx + dim.width - 1, y - arrow_len, 1, 1);
|
||||
write_hline_lm(xx, xx + dim.width - 1, y + arrow_len - 2, 1, 1);
|
||||
write_vline_lm(xx + dim.width - 1, y - arrow_len, y + arrow_len - 2, 1, 1);
|
||||
}
|
||||
else
|
||||
{
|
||||
} else {
|
||||
write_hline_lm(xx, xx - dim.width - 1, y - arrow_len, 1, 1);
|
||||
write_hline_lm(xx, xx - dim.width - 1, y + arrow_len - 2, 1, 1);
|
||||
write_vline_lm(xx - dim.width - 1, y - arrow_len, y + arrow_len - 2, 1, 1);
|
||||
}
|
||||
// Draw the text.
|
||||
if(halign == -1)
|
||||
if (halign == -1)
|
||||
write_string(temp, xx, y, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_LEFT, 0, 0);
|
||||
else
|
||||
write_string(temp, xx, y, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_RIGHT, 0, 0);
|
||||
// Then, add a slow cut off on the edges, so the text doesn't sharply
|
||||
// disappear. We simply clear the areas above and below the ticker, and we
|
||||
// use little markers on the edges.
|
||||
if(halign == -1)
|
||||
{
|
||||
write_filled_rectangle_lm(majtick_end + text_x_spacing, y + (height / 2) - (font_info.height / 2), max_text_y - boundtick_start, font_info.height, 0, 0);
|
||||
write_filled_rectangle_lm(majtick_end + text_x_spacing, y - (height / 2) - (font_info.height / 2), max_text_y - boundtick_start, font_info.height, 0, 0);
|
||||
}
|
||||
else
|
||||
{
|
||||
if (halign == -1) {
|
||||
write_filled_rectangle_lm(majtick_end + text_x_spacing, y + (height / 2) - (font_info.height / 2), max_text_y - boundtick_start, font_info.height, 0,
|
||||
0);
|
||||
write_filled_rectangle_lm(majtick_end + text_x_spacing, y - (height / 2) - (font_info.height / 2), max_text_y - boundtick_start, font_info.height, 0,
|
||||
0);
|
||||
} else {
|
||||
write_filled_rectangle_lm(majtick_end - text_x_spacing - max_text_y, y + (height / 2) - (font_info.height / 2), max_text_y, font_info.height, 0, 0);
|
||||
write_filled_rectangle_lm(majtick_end - text_x_spacing - max_text_y, y - (height / 2) - (font_info.height / 2), max_text_y, font_info.height, 0, 0);
|
||||
}
|
||||
@ -1826,42 +1749,43 @@ void hud_draw_linear_compass(int v, int range, int width, int x, int y, int mint
|
||||
textoffset = 8;
|
||||
int r, style, rr, xs; // rv,
|
||||
int range_2 = range / 2;
|
||||
for(r = -range_2; r <= +range_2; r++)
|
||||
{
|
||||
for (r = -range_2; r <= +range_2; r++) {
|
||||
style = 0;
|
||||
rr = (v + r + 360) % 360; // normalise range for modulo, add to move compass track
|
||||
//rv = -rr + range_2; // for number display
|
||||
if(rr % majtick_step == 0)
|
||||
if (rr % majtick_step == 0)
|
||||
style = 1; // major tick
|
||||
else if(rr % mintick_step == 0)
|
||||
else if (rr % mintick_step == 0)
|
||||
style = 2; // minor tick
|
||||
if(style)
|
||||
{
|
||||
if (style) {
|
||||
// Calculate x position.
|
||||
xs = ((long int)(r * width) / (long int)range) + x;
|
||||
xs = ((long int) (r * width) / (long int) range) + x;
|
||||
// Draw it.
|
||||
if(style == 1)
|
||||
{
|
||||
if (style == 1) {
|
||||
write_vline_outlined(xs, majtick_start, majtick_end, 2, 2, 0, 1);
|
||||
// Draw heading above this tick.
|
||||
// If it's not one of north, south, east, west, draw the heading.
|
||||
// Otherwise, draw one of the identifiers.
|
||||
if(rr % 90 != 0)
|
||||
{
|
||||
if (rr % 90 != 0) {
|
||||
// We abbreviate heading to two digits. This has the side effect of being easy to compute.
|
||||
headingstr[0] = '0' + (rr / 100);
|
||||
headingstr[1] = '0' + ((rr / 10) % 10);
|
||||
headingstr[2] = 0;
|
||||
headingstr[3] = 0; // nul to terminate
|
||||
}
|
||||
else
|
||||
{
|
||||
switch(rr)
|
||||
{
|
||||
case 0: headingstr[0] = 'N'; break;
|
||||
case 90: headingstr[0] = 'E'; break;
|
||||
case 180: headingstr[0] = 'S'; break;
|
||||
case 270: headingstr[0] = 'W'; break;
|
||||
} else {
|
||||
switch (rr) {
|
||||
case 0:
|
||||
headingstr[0] = 'N';
|
||||
break;
|
||||
case 90:
|
||||
headingstr[0] = 'E';
|
||||
break;
|
||||
case 180:
|
||||
headingstr[0] = 'S';
|
||||
break;
|
||||
case 270:
|
||||
headingstr[0] = 'W';
|
||||
break;
|
||||
}
|
||||
headingstr[1] = 0;
|
||||
headingstr[2] = 0;
|
||||
@ -1869,8 +1793,7 @@ void hud_draw_linear_compass(int v, int range, int width, int x, int y, int mint
|
||||
}
|
||||
// +1 fudge...!
|
||||
write_string(headingstr, xs + 1, majtick_start + textoffset, 1, 0, TEXT_VA_MIDDLE, TEXT_HA_CENTER, 0, 1);
|
||||
}
|
||||
else if(style == 2)
|
||||
} else if (style == 2)
|
||||
write_vline_outlined(xs, mintick_start, mintick_end, 2, 2, 0, 1);
|
||||
}
|
||||
}
|
||||
@ -1886,226 +1809,201 @@ void hud_draw_linear_compass(int v, int range, int width, int x, int y, int mint
|
||||
headingstr[1] = '0' + ((v / 10) % 10);
|
||||
headingstr[2] = '0' + (v % 10);
|
||||
headingstr[3] = 0;
|
||||
write_string(headingstr, x + 1, majtick_start + textoffset+2, 0, 0, TEXT_VA_MIDDLE, TEXT_HA_CENTER, 1, 3);
|
||||
write_string(headingstr, x + 1, majtick_start + textoffset + 2, 0, 0, TEXT_VA_MIDDLE, TEXT_HA_CENTER, 1, 3);
|
||||
}
|
||||
// CORE draw routines end here
|
||||
|
||||
void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y, int16_t size )
|
||||
void draw_artificial_horizon(float angle, float pitch, int16_t l_x, int16_t l_y, int16_t size)
|
||||
{
|
||||
float alpha;
|
||||
uint8_t vertical=0,horizontal=0;
|
||||
int16_t x1,x2;
|
||||
int16_t y1,y2;
|
||||
int16_t refx,refy;
|
||||
alpha=DEG2RAD(angle);
|
||||
refx=l_x + size/2;
|
||||
refy=l_y + size/2;
|
||||
uint8_t vertical = 0, horizontal = 0;
|
||||
int16_t x1, x2;
|
||||
int16_t y1, y2;
|
||||
int16_t refx, refy;
|
||||
alpha = DEG2RAD(angle);
|
||||
refx = l_x + size / 2;
|
||||
refy = l_y + size / 2;
|
||||
|
||||
//
|
||||
float k=0;
|
||||
float dx = sinf(alpha)*(pitch/90.0f*(size/2));
|
||||
float dy = cosf(alpha)*(pitch/90.0f*(size/2));
|
||||
int16_t x0 = (size/2)-dx;
|
||||
int16_t y0 = (size/2)+dy;
|
||||
float k = 0;
|
||||
float dx = sinf(alpha) * (pitch / 90.0f * (size / 2));
|
||||
float dy = cosf(alpha) * (pitch / 90.0f * (size / 2));
|
||||
int16_t x0 = (size / 2) - dx;
|
||||
int16_t y0 = (size / 2) + dy;
|
||||
// calculate the line function
|
||||
if((angle != 90) && (angle != -90))
|
||||
{
|
||||
if ((angle != 90) && (angle != -90)) {
|
||||
k = tanf(alpha);
|
||||
vertical = 0;
|
||||
if(k==0)
|
||||
{
|
||||
horizontal=1;
|
||||
if (k == 0) {
|
||||
horizontal = 1;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
} else {
|
||||
vertical = 1;
|
||||
}
|
||||
|
||||
// crossing point of line
|
||||
if(!vertical && !horizontal)
|
||||
{
|
||||
if (!vertical && !horizontal) {
|
||||
// y-y0=k(x-x0)
|
||||
int16_t x=0;
|
||||
int16_t y=k*(x-x0)+y0;
|
||||
int16_t x = 0;
|
||||
int16_t y = k * (x - x0) + y0;
|
||||
// find right crossing point
|
||||
x1=x;
|
||||
y1=y;
|
||||
if(y<0)
|
||||
{
|
||||
y1=0;
|
||||
x1=((y1-y0)+k*x0)/k;
|
||||
x1 = x;
|
||||
y1 = y;
|
||||
if (y < 0) {
|
||||
y1 = 0;
|
||||
x1 = ((y1 - y0) + k * x0) / k;
|
||||
}
|
||||
if(y>size)
|
||||
{
|
||||
y1=size;
|
||||
x1=((y1-y0)+k*x0)/k;
|
||||
if (y > size) {
|
||||
y1 = size;
|
||||
x1 = ((y1 - y0) + k * x0) / k;
|
||||
}
|
||||
// left crossing point
|
||||
x=size;
|
||||
y=k*(x-x0)+y0;
|
||||
x2=x;
|
||||
y2=y;
|
||||
if(y<0)
|
||||
{
|
||||
y2=0;
|
||||
x2=((y2-y0)+k*x0)/k;
|
||||
x = size;
|
||||
y = k * (x - x0) + y0;
|
||||
x2 = x;
|
||||
y2 = y;
|
||||
if (y < 0) {
|
||||
y2 = 0;
|
||||
x2 = ((y2 - y0) + k * x0) / k;
|
||||
}
|
||||
if(y>size)
|
||||
{
|
||||
y2=size;
|
||||
x2=((y2-y0)+k*x0)/k;
|
||||
if (y > size) {
|
||||
y2 = size;
|
||||
x2 = ((y2 - y0) + k * x0) / k;
|
||||
}
|
||||
// move to location
|
||||
// horizon line
|
||||
write_line_outlined(x1+l_x,y1+l_y,x2+l_x,y2+l_y,0,0,0,1);
|
||||
write_line_outlined(x1 + l_x, y1 + l_y, x2 + l_x, y2 + l_y, 0, 0, 0, 1);
|
||||
//fill
|
||||
if(angle<=0 && angle>-90)
|
||||
{
|
||||
if (angle <= 0 && angle > -90) {
|
||||
//write_string("1", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
|
||||
for(int i=y2;i<size;i++)
|
||||
{
|
||||
x2=((i-y0)+k*x0)/k;
|
||||
if(x2>size)
|
||||
x2=size;
|
||||
if(x2<0)
|
||||
x2=0;
|
||||
write_hline_lm(x2+l_x,size+l_x,i+l_y,1,1);
|
||||
for (int i = y2; i < size; i++) {
|
||||
x2 = ((i - y0) + k * x0) / k;
|
||||
if (x2 > size) {
|
||||
x2 = size;
|
||||
}
|
||||
if (x2 < 0) {
|
||||
x2 = 0;
|
||||
}
|
||||
else if(angle<-90)
|
||||
{
|
||||
write_hline_lm(x2 + l_x, size + l_x, i + l_y, 1, 1);
|
||||
}
|
||||
} else if (angle < -90) {
|
||||
//write_string("2", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
|
||||
for(int i=0;i<y2;i++)
|
||||
{
|
||||
x2=((i-y0)+k*x0)/k;
|
||||
if(x2>size)
|
||||
x2=size;
|
||||
if(x2<0)
|
||||
x2=0;
|
||||
write_hline_lm(size+l_x,x2+l_x,i+l_y,1,1);
|
||||
for (int i = 0; i < y2; i++) {
|
||||
x2 = ((i - y0) + k * x0) / k;
|
||||
if (x2 > size) {
|
||||
x2 = size;
|
||||
}
|
||||
if (x2 < 0) {
|
||||
x2 = 0;
|
||||
}
|
||||
else if(angle>0 && angle<90)
|
||||
{
|
||||
write_hline_lm(size + l_x, x2 + l_x, i + l_y, 1, 1);
|
||||
}
|
||||
} else if (angle > 0 && angle < 90) {
|
||||
//write_string("3", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
|
||||
for(int i=y1;i<size;i++)
|
||||
{
|
||||
x2=((i-y0)+k*x0)/k;
|
||||
if(x2>size)
|
||||
x2=size;
|
||||
if(x2<0)
|
||||
x2=0;
|
||||
write_hline_lm(0+l_x,x2+l_x,i+l_y,1,1);
|
||||
for (int i = y1; i < size; i++) {
|
||||
x2 = ((i - y0) + k * x0) / k;
|
||||
if (x2 > size) {
|
||||
x2 = size;
|
||||
}
|
||||
if (x2 < 0) {
|
||||
x2 = 0;
|
||||
}
|
||||
else if(angle>90)
|
||||
{
|
||||
write_hline_lm(0 + l_x, x2 + l_x, i + l_y, 1, 1);
|
||||
}
|
||||
} else if (angle > 90) {
|
||||
//write_string("4", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
|
||||
for(int i=0;i<y1;i++)
|
||||
{
|
||||
x2=((i-y0)+k*x0)/k;
|
||||
if(x2>size)
|
||||
x2=size;
|
||||
if(x2<0)
|
||||
x2=0;
|
||||
write_hline_lm(x2+l_x,0+l_x,i+l_y,1,1);
|
||||
for (int i = 0; i < y1; i++) {
|
||||
x2 = ((i - y0) + k * x0) / k;
|
||||
if (x2 > size) {
|
||||
x2 = size;
|
||||
}
|
||||
if (x2 < 0) {
|
||||
x2 = 0;
|
||||
}
|
||||
write_hline_lm(x2 + l_x, 0 + l_x, i + l_y, 1, 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(vertical)
|
||||
{
|
||||
} else if (vertical) {
|
||||
// horizon line
|
||||
write_line_outlined(x0+l_x,0+l_y,x0+l_x,size+l_y,0,0,0,1);
|
||||
if(angle==90)
|
||||
{
|
||||
write_line_outlined(x0 + l_x, 0 + l_y, x0 + l_x, size + l_y, 0, 0, 0, 1);
|
||||
if (angle == 90) {
|
||||
//write_string("5", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
|
||||
for(int i=0;i<size;i++)
|
||||
{
|
||||
write_hline_lm(0+l_x,x0+l_x,i+l_y,1,1);
|
||||
for (int i = 0; i < size; i++) {
|
||||
write_hline_lm(0 + l_x, x0 + l_x, i + l_y, 1, 1);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
} else {
|
||||
//write_string("6", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
|
||||
for(int i=0;i<size;i++)
|
||||
{
|
||||
write_hline_lm(size+l_x,x0+l_x,i+l_y,1,1);
|
||||
for (int i = 0; i < size; i++) {
|
||||
write_hline_lm(size + l_x, x0 + l_x, i + l_y, 1, 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(horizontal)
|
||||
{
|
||||
} else if (horizontal) {
|
||||
// horizon line
|
||||
write_hline_outlined(0+l_x,size+l_x,y0+l_y,0,0,0,1);
|
||||
if(angle<0)
|
||||
{
|
||||
write_hline_outlined(0 + l_x, size + l_x, y0 + l_y, 0, 0, 0, 1);
|
||||
if (angle < 0) {
|
||||
//write_string("7", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
|
||||
for(int i=0;i<y0;i++)
|
||||
{
|
||||
write_hline_lm(0+l_x,size+l_x,i+l_y,1,1);
|
||||
for (int i = 0; i < y0; i++) {
|
||||
write_hline_lm(0 + l_x, size + l_x, i + l_y, 1, 1);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
} else {
|
||||
//write_string("8", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
|
||||
for(int i=y0;i<size;i++)
|
||||
{
|
||||
write_hline_lm(0+l_x,size+l_x,i+l_y,1,1);
|
||||
for (int i = y0; i < size; i++) {
|
||||
write_hline_lm(0 + l_x, size + l_x, i + l_y, 1, 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//sides
|
||||
write_line_outlined(l_x,l_y,l_x,l_y+size,0,0,0,1);
|
||||
write_line_outlined(l_x+size,l_y,l_x+size,l_y+size,0,0,0,1);
|
||||
write_line_outlined(l_x, l_y, l_x, l_y + size, 0, 0, 0, 1);
|
||||
write_line_outlined(l_x + size, l_y, l_x + size, l_y +s ize, 0, 0, 0, 1);
|
||||
//plane
|
||||
write_line_outlined(refx-5,refy,refx+6,refy,0,0,0,1);
|
||||
write_line_outlined(refx,refy,refx,refy-3,0,0,0,1);
|
||||
write_line_outlined(refx - 5, refy, refx + 6, refy, 0, 0, 0, 1);
|
||||
write_line_outlined(refx, refy, refx, refy - 3, 0, 0, 0, 1);
|
||||
}
|
||||
|
||||
|
||||
void introText(){
|
||||
write_string("ver 0.2", APPLY_HDEADBAND((GRAPHICS_RIGHT/2)),APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
|
||||
void introText()
|
||||
{
|
||||
write_string("ver 0.2", APPLY_HDEADBAND((GRAPHICS_RIGHT / 2)), APPLY_VDEADBAND(GRAPHICS_BOTTOM - 10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_CENTER, 0, 3);
|
||||
}
|
||||
|
||||
void introGraphics() {
|
||||
void introGraphics()
|
||||
{
|
||||
/* logo */
|
||||
int image=0;
|
||||
int image = 0;
|
||||
struct splashEntry splash_info;
|
||||
splash_info = splash[image];
|
||||
|
||||
copyimage(APPLY_HDEADBAND(GRAPHICS_RIGHT/2-(splash_info.width)/2), APPLY_VDEADBAND(GRAPHICS_BOTTOM/2-(splash_info.height)/2),image);
|
||||
copyimage(APPLY_HDEADBAND(GRAPHICS_RIGHT / 2 - (splash_info.width) / 2), APPLY_VDEADBAND(GRAPHICS_BOTTOM / 2 - (splash_info.height) / 2), image);
|
||||
|
||||
/* frame */
|
||||
drawBox(APPLY_HDEADBAND(0),APPLY_VDEADBAND(0),APPLY_HDEADBAND(GRAPHICS_RIGHT-8),APPLY_VDEADBAND(GRAPHICS_BOTTOM));
|
||||
drawBox(APPLY_HDEADBAND(0), APPLY_VDEADBAND(0), APPLY_HDEADBAND(GRAPHICS_RIGHT-8), APPLY_VDEADBAND(GRAPHICS_BOTTOM));
|
||||
|
||||
// Must mask out last half-word because SPI keeps clocking it out otherwise
|
||||
for (uint32_t i = 0; i < 8; i++) {
|
||||
write_vline( draw_buffer_level,GRAPHICS_WIDTH_REAL-i-1,0,GRAPHICS_HEIGHT_REAL-1,0);
|
||||
write_vline( draw_buffer_mask,GRAPHICS_WIDTH_REAL-i-1,0,GRAPHICS_HEIGHT_REAL-1,0);
|
||||
write_vline(draw_buffer_level, GRAPHICS_WIDTH_REAL - i - 1, 0, GRAPHICS_HEIGHT_REAL - 1, 0);
|
||||
write_vline(draw_buffer_mask, GRAPHICS_WIDTH_REAL - i - 1, 0, GRAPHICS_HEIGHT_REAL - 1, 0);
|
||||
}
|
||||
}
|
||||
|
||||
void calcHomeArrow(int16_t m_yaw)
|
||||
{
|
||||
HomeLocationData home;
|
||||
HomeLocationGet (&home);
|
||||
HomeLocationGet(&home);
|
||||
GPSPositionData gpsData;
|
||||
GPSPositionGet (&gpsData);
|
||||
GPSPositionGet(&gpsData);
|
||||
|
||||
/** http://www.movable-type.co.uk/scripts/latlong.html **/
|
||||
float lat1, lat2, lon1, lon2, a, c, d, x, y, brng, u2g;
|
||||
float elevation;
|
||||
float gcsAlt=home.Altitude; // Home MSL altitude
|
||||
float uavAlt=gpsData.Altitude; // UAV MSL altitude
|
||||
float dAlt=uavAlt-gcsAlt; // Altitude difference
|
||||
float gcsAlt = home.Altitude; // Home MSL altitude
|
||||
float uavAlt = gpsData.Altitude; // UAV MSL altitude
|
||||
float dAlt = uavAlt - gcsAlt; // Altitude difference
|
||||
|
||||
// Convert to radians
|
||||
lat1 = DEG2RAD(home.Latitude)/10000000.0f; // Home lat
|
||||
lon1 = DEG2RAD(home.Longitude)/10000000.0f; // Home lon
|
||||
lat2 = DEG2RAD(gpsData.Latitude)/10000000.0f; // UAV lat
|
||||
lon2 = DEG2RAD(gpsData.Longitude)/10000000.0f; // UAV lon
|
||||
lat1 = DEG2RAD(home.Latitude) / 10000000.0f; // Home lat
|
||||
lon1 = DEG2RAD(home.Longitude) / 10000000.0f; // Home lon
|
||||
lat2 = DEG2RAD(gpsData.Latitude) / 10000000.0f; // UAV lat
|
||||
lon2 = DEG2RAD(gpsData.Longitude) / 10000000.0f; // UAV lon
|
||||
|
||||
// Bearing
|
||||
/**
|
||||
@ -2114,16 +2012,18 @@ void calcHomeArrow(int16_t m_yaw)
|
||||
Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
|
||||
var brng = Math.atan2(y, x).toDeg();
|
||||
**/
|
||||
y = sinf(lon2-lon1) * cosf(lat2);
|
||||
x = cosf(lat1) * sinf(lat2) - sinf(lat1) * cosf(lat2) * cosf(lon2-lon1);
|
||||
y = sinf(lon2 - lon1) * cosf(lat2);
|
||||
x = cosf(lat1) * sinf(lat2) - sinf(lat1) * cosf(lat2) * cosf(lon2 - lon1);
|
||||
brng = RAD2DEG(atan2f(y,x));
|
||||
if(brng<0)
|
||||
brng+=360;
|
||||
if (brng < 0) {
|
||||
brng += 360;
|
||||
}
|
||||
|
||||
// yaw corrected bearing, needs compass
|
||||
u2g=brng-180-m_yaw;
|
||||
if(u2g<0)
|
||||
u2g+=360;
|
||||
u2g = brng - 180 - m_yaw;
|
||||
if (u2g < 0) {
|
||||
u2g += 360;
|
||||
}
|
||||
|
||||
// Haversine formula for distance
|
||||
/**
|
||||
@ -2136,60 +2036,58 @@ void calcHomeArrow(int16_t m_yaw)
|
||||
var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
|
||||
var d = R * c;
|
||||
**/
|
||||
a = sinf((lat2-lat1)/2) * sinf((lat2-lat1)/2) +
|
||||
cosf(lat1) * cosf(lat2) *
|
||||
sinf((lon2-lon1)/2) * sinf((lon2-lon1)/2);
|
||||
c = 2 * atan2f(sqrtf(a), sqrtf(1-a));
|
||||
a = sinf((lat2 - lat1) / 2) * sinf((lat2 - lat1) / 2) + cosf(lat1) * cosf(lat2) * sinf((lon2 - lon1) / 2) * sinf((lon2 - lon1) / 2);
|
||||
c = 2 * atan2f(sqrtf(a), sqrtf(1 - a));
|
||||
d = 6371 * 1000 * c;
|
||||
|
||||
// Elevation v depends servo direction
|
||||
if(d!=0)
|
||||
elevation = 90-RAD2DEG(atanf(dAlt/d));
|
||||
if (d != 0)
|
||||
elevation = 90 - RAD2DEG(atanf(dAlt/d));
|
||||
else
|
||||
elevation = 0;
|
||||
//! TODO: sanity check
|
||||
|
||||
char temp[50]={0};
|
||||
sprintf(temp,"hea:%d",(int)brng);
|
||||
char temp[50] =
|
||||
{ 0 };
|
||||
sprintf(temp, "hea:%d", (int) brng);
|
||||
write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
|
||||
sprintf(temp,"ele:%d",(int)elevation);
|
||||
sprintf(temp, "ele:%d", (int) elevation);
|
||||
write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
|
||||
sprintf(temp,"dis:%d",(int)d);
|
||||
sprintf(temp, "dis:%d", (int) d);
|
||||
write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
|
||||
sprintf(temp,"u2g:%d",(int)u2g);
|
||||
sprintf(temp, "u2g:%d", (int) u2g);
|
||||
write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT/2-30), APPLY_VDEADBAND(30+10+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
|
||||
|
||||
sprintf(temp,"%c%c",(int)(u2g/22.5f)*2+0x90,(int)(u2g/22.5f)*2+0x91);
|
||||
sprintf(temp, "%c%c", (int) (u2g / 22.5f) * 2 + 0x90, (int) (u2g / 22.5f) * 2 + 0x91);
|
||||
write_string(temp, APPLY_HDEADBAND(250), APPLY_VDEADBAND(40+10+10), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3);
|
||||
}
|
||||
|
||||
int lama=10;
|
||||
int lama = 10;
|
||||
int lama_loc[2][30];
|
||||
|
||||
void lamas(void)
|
||||
{
|
||||
char temp[10]={0};
|
||||
char temp[10] =
|
||||
{ 0 };
|
||||
lama++;
|
||||
if(lama%10==0)
|
||||
{
|
||||
for(int z=0; z<30;z++)
|
||||
{
|
||||
if (lama % 10 == 0) {
|
||||
for (int z = 0; z < 30; z++) {
|
||||
|
||||
lama_loc[0][z]=rand()%(GRAPHICS_RIGHT-10);
|
||||
lama_loc[1][z]=rand()%(GRAPHICS_BOTTOM-10);
|
||||
lama_loc[0][z] = rand() % (GRAPHICS_RIGHT - 10);
|
||||
lama_loc[1][z] = rand() % (GRAPHICS_BOTTOM - 10);
|
||||
}
|
||||
}
|
||||
for(int z=0; z<30;z++)
|
||||
{
|
||||
sprintf(temp,"%c",0xe8+(lama_loc[0][z]%2));
|
||||
write_string(temp,APPLY_HDEADBAND(lama_loc[0][z]),APPLY_VDEADBAND(lama_loc[1][z]), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
|
||||
for (int z = 0; z < 30; z++) {
|
||||
sprintf(temp, "%c", 0xe8 + (lama_loc[0][z] % 2));
|
||||
write_string(temp, APPLY_HDEADBAND(lama_loc[0][z]), APPLY_VDEADBAND(lama_loc[1][z]), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
|
||||
}
|
||||
}
|
||||
|
||||
//main draw function
|
||||
void updateGraphics() {
|
||||
void updateGraphics()
|
||||
{
|
||||
OsdSettingsData OsdSettings;
|
||||
OsdSettingsGet (&OsdSettings);
|
||||
OsdSettingsGet(&OsdSettings);
|
||||
AttitudeActualData attitude;
|
||||
AttitudeActualGet(&attitude);
|
||||
GPSPositionData gpsData;
|
||||
@ -2199,36 +2097,36 @@ void updateGraphics() {
|
||||
BaroAltitudeData baro;
|
||||
BaroAltitudeGet(&baro);
|
||||
|
||||
PIOS_Servo_Set(0,OsdSettings.White);
|
||||
PIOS_Servo_Set(1,OsdSettings.Black);
|
||||
PIOS_Servo_Set(0, OsdSettings.White);
|
||||
PIOS_Servo_Set(1, OsdSettings.Black);
|
||||
|
||||
switch (OsdSettings.Screen) {
|
||||
case 0: // Dave simple
|
||||
{
|
||||
if(home.Set == HOMELOCATION_SET_FALSE)
|
||||
{
|
||||
char temps[20]={0};
|
||||
sprintf(temps,"HOME NOT SET");
|
||||
if (home.Set == HOMELOCATION_SET_FALSE) {
|
||||
char temps[20] =
|
||||
{ 0 };
|
||||
sprintf(temps, "HOME NOT SET");
|
||||
//printTextFB(x,y,temp);
|
||||
write_string(temps, APPLY_HDEADBAND(GRAPHICS_RIGHT/2), (GRAPHICS_BOTTOM/2), 0, 0, TEXT_VA_TOP, TEXT_HA_CENTER, 0, 3);
|
||||
write_string(temps, APPLY_HDEADBAND(GRAPHICS_RIGHT/2), (GRAPHICS_BOTTOM / 2), 0, 0, TEXT_VA_TOP, TEXT_HA_CENTER, 0, 3);
|
||||
}
|
||||
|
||||
|
||||
char temp[50]={0};
|
||||
char temp[50] =
|
||||
{ 0 };
|
||||
memset(temp, ' ', 40);
|
||||
sprintf(temp,"Lat:%11.7f",gpsData.Latitude/10000000.0f);
|
||||
sprintf(temp, "Lat:%11.7f", gpsData.Latitude / 10000000.0f);
|
||||
write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM-30), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3);
|
||||
sprintf(temp,"Lon:%11.7f",gpsData.Longitude/10000000.0f);
|
||||
sprintf(temp, "Lon:%11.7f", gpsData.Longitude / 10000000.0f);
|
||||
write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(GRAPHICS_BOTTOM-10), 0, 0, TEXT_VA_BOTTOM, TEXT_HA_LEFT, 0, 3);
|
||||
sprintf(temp,"Sat:%d",(int)gpsData.Satellites);
|
||||
sprintf(temp, "Sat:%d", (int) gpsData.Satellites);
|
||||
write_string(temp, APPLY_HDEADBAND(GRAPHICS_RIGHT-40), APPLY_VDEADBAND(30), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
|
||||
|
||||
/* Print ADC voltage FLIGHT*/
|
||||
sprintf(temp,"V:%5.2fV",(PIOS_ADC_PinGet(2)*3*6.1f/4096));
|
||||
sprintf(temp, "V:%5.2fV", (PIOS_ADC_PinGet(2) * 3 * 6.1f / 4096));
|
||||
write_string(temp, APPLY_HDEADBAND(20), APPLY_VDEADBAND(20), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 3);
|
||||
|
||||
if(gpsData.Heading>180)
|
||||
calcHomeArrow((int16_t)(gpsData.Heading-360));
|
||||
if (gpsData.Heading > 180)
|
||||
calcHomeArrow((int16_t)(gpsData.Heading - 360));
|
||||
else
|
||||
calcHomeArrow((int16_t)(gpsData.Heading));
|
||||
}
|
||||
@ -2259,63 +2157,61 @@ void updateGraphics() {
|
||||
angleC+=2;*/
|
||||
|
||||
// GPS HACK
|
||||
if(gpsData.Heading>180)
|
||||
calcHomeArrow((int16_t)(gpsData.Heading-360));
|
||||
if (gpsData.Heading > 180)
|
||||
calcHomeArrow((int16_t)(gpsData.Heading - 360));
|
||||
else
|
||||
calcHomeArrow((int16_t)(gpsData.Heading));
|
||||
|
||||
/* Draw Attitude Indicator */
|
||||
if(OsdSettings.Attitude == OSDSETTINGS_ATTITUDE_ENABLED)
|
||||
{
|
||||
drawAttitude(APPLY_HDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_X]),APPLY_VDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_Y]),attitude.Pitch,attitude.Roll,96);
|
||||
if (OsdSettings.Attitude == OSDSETTINGS_ATTITUDE_ENABLED) {
|
||||
drawAttitude(APPLY_HDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_X]),
|
||||
APPLY_VDEADBAND(OsdSettings.AttitudeSetup[OSDSETTINGS_ATTITUDESETUP_Y]), attitude.Pitch, attitude.Roll, 96);
|
||||
}
|
||||
//write_string("Hello OP-OSD", 60, 12, 1, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 0);
|
||||
//printText16( 60, 12,"Hello OP-OSD");
|
||||
|
||||
char temp[50]={0};
|
||||
char temp[50] =
|
||||
{ 0 };
|
||||
memset(temp, ' ', 40);
|
||||
sprintf(temp,"Lat:%11.7f",gpsData.Latitude/10000000.0f);
|
||||
sprintf(temp, "Lat:%11.7f", gpsData.Latitude / 10000000.0f);
|
||||
write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
|
||||
sprintf(temp,"Lon:%11.7f",gpsData.Longitude/10000000.0f);
|
||||
sprintf(temp, "Lon:%11.7f", gpsData.Longitude / 10000000.0f);
|
||||
write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
|
||||
sprintf(temp,"Fix:%d",(int)gpsData.Status);
|
||||
sprintf(temp, "Fix:%d", (int) gpsData.Status);
|
||||
write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
|
||||
sprintf(temp,"Sat:%d",(int)gpsData.Satellites);
|
||||
sprintf(temp, "Sat:%d", (int) gpsData.Satellites);
|
||||
write_string(temp, APPLY_HDEADBAND(5), APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_LEFT, 0, 2);
|
||||
|
||||
|
||||
/* Print RTC time */
|
||||
if(OsdSettings.Time == OSDSETTINGS_TIME_ENABLED)
|
||||
{
|
||||
printTime(APPLY_HDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_X]),APPLY_VDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_Y]));
|
||||
if (OsdSettings.Time == OSDSETTINGS_TIME_ENABLED) {
|
||||
printTime(APPLY_HDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_X]), APPLY_VDEADBAND(OsdSettings.TimeSetup[OSDSETTINGS_TIMESETUP_Y]));
|
||||
}
|
||||
|
||||
/* Print Number of detected video Lines */
|
||||
sprintf(temp,"Lines:%4d",PIOS_Video_GetOSDLines());
|
||||
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)),APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
|
||||
sprintf(temp, "Lines:%4d", PIOS_Video_GetOSDLines());
|
||||
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(5), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
|
||||
|
||||
/* Print ADC voltage */
|
||||
//sprintf(temp,"Rssi:%4dV",(int)(PIOS_ADC_PinGet(4)*3000/4096));
|
||||
//write_string(temp, (GRAPHICS_WIDTH_REAL - 2),15, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
|
||||
sprintf(temp,"Rssi:%4.2fV",(PIOS_ADC_PinGet(5)*3.0f/4096.0f));
|
||||
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)),APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
|
||||
sprintf(temp, "Rssi:%4.2fV", (PIOS_ADC_PinGet(5) * 3.0f / 4096.0f));
|
||||
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(15), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
|
||||
|
||||
/* Print CPU temperature */
|
||||
sprintf(temp,"Temp:%4.2fC",(PIOS_ADC_PinGet(3)*0.29296875f-264));
|
||||
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)),APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
|
||||
sprintf(temp, "Temp:%4.2fC", (PIOS_ADC_PinGet(3) * 0.29296875f - 264));
|
||||
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(25), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
|
||||
|
||||
/* Print ADC voltage FLIGHT*/
|
||||
sprintf(temp,"FltV:%4.2fV",(PIOS_ADC_PinGet(2)*3.0f*6.1f/4096.0f));
|
||||
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)),APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
|
||||
sprintf(temp, "FltV:%4.2fV", (PIOS_ADC_PinGet(2) * 3.0f * 6.1f / 4096.0f));
|
||||
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(35), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
|
||||
|
||||
/* Print ADC voltage VIDEO*/
|
||||
sprintf(temp,"VidV:%4.2fV",(PIOS_ADC_PinGet(4)*3.0f*6.1f/4096.0f));
|
||||
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)),APPLY_VDEADBAND(45), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
|
||||
sprintf(temp, "VidV:%4.2fV", (PIOS_ADC_PinGet(4) * 3.0f * 6.1f / 4096.0f));
|
||||
write_string(temp, APPLY_HDEADBAND((GRAPHICS_RIGHT - 8)), APPLY_VDEADBAND(45), 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
|
||||
|
||||
/* Print ADC voltage RSSI */
|
||||
//sprintf(temp,"Curr:%4dA",(int)(PIOS_ADC_PinGet(0)*300*61/4096));
|
||||
//write_string(temp, (GRAPHICS_WIDTH_REAL - 2),60, 0, 0, TEXT_VA_TOP, TEXT_HA_RIGHT, 0, 2);
|
||||
|
||||
/* Draw Battery Gauge */
|
||||
/*m_batt++;
|
||||
uint8_t dir=3;
|
||||
@ -2339,26 +2235,21 @@ void updateGraphics() {
|
||||
|
||||
//drawAltitude(200,50,m_alt,dir);
|
||||
|
||||
|
||||
//drawArrow(96,GRAPHICS_HEIGHT_REAL/2,angleB,32);
|
||||
|
||||
// Draw airspeed (left side.)
|
||||
if(OsdSettings.Speed == OSDSETTINGS_SPEED_ENABLED)
|
||||
{
|
||||
hud_draw_vertical_scale((int)gpsData.Groundspeed, 100, -1, APPLY_HDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_X]),
|
||||
if (OsdSettings.Speed == OSDSETTINGS_SPEED_ENABLED) {
|
||||
hud_draw_vertical_scale((int) gpsData.Groundspeed, 100, -1, APPLY_HDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_X]),
|
||||
APPLY_VDEADBAND(OsdSettings.SpeedSetup[OSDSETTINGS_SPEEDSETUP_Y]), 100, 10, 20, 7, 12, 15, 1000, HUD_VSCALE_FLAG_NO_NEGATIVE);
|
||||
}
|
||||
// Draw altimeter (right side.)
|
||||
if(OsdSettings.Altitude == OSDSETTINGS_ALTITUDE_ENABLED)
|
||||
{
|
||||
hud_draw_vertical_scale((int)gpsData.Altitude, 200, +1, APPLY_HDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_X]),
|
||||
if (OsdSettings.Altitude == OSDSETTINGS_ALTITUDE_ENABLED) {
|
||||
hud_draw_vertical_scale((int) gpsData.Altitude, 200, +1, APPLY_HDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_X]),
|
||||
APPLY_VDEADBAND(OsdSettings.AltitudeSetup[OSDSETTINGS_ALTITUDESETUP_Y]), 100, 20, 100, 7, 12, 15, 500, 0);
|
||||
}
|
||||
// Draw compass.
|
||||
if(OsdSettings.Heading == OSDSETTINGS_HEADING_ENABLED)
|
||||
{
|
||||
if(attitude.Yaw<0) {
|
||||
hud_draw_linear_compass(360+attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]),
|
||||
if (OsdSettings.Heading == OSDSETTINGS_HEADING_ENABLED) {
|
||||
if (attitude.Yaw < 0) {
|
||||
hud_draw_linear_compass(360 + attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]),
|
||||
APPLY_VDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_Y]), 15, 30, 7, 12, 0);
|
||||
} else {
|
||||
hud_draw_linear_compass(attitude.Yaw, 150, 120, APPLY_HDEADBAND(OsdSettings.HeadingSetup[OSDSETTINGS_HEADINGSETUP_X]),
|
||||
@ -2369,20 +2260,16 @@ void updateGraphics() {
|
||||
break;
|
||||
case 2:
|
||||
{
|
||||
int size=64;
|
||||
int x=((GRAPHICS_RIGHT/2)-(size/2)),y=(GRAPHICS_BOTTOM-size-2);
|
||||
draw_artificial_horizon(-attitude.Roll,attitude.Pitch,APPLY_HDEADBAND(x),APPLY_VDEADBAND(y),size);
|
||||
hud_draw_vertical_scale((int)gpsData.Groundspeed, 20, +1, APPLY_HDEADBAND(GRAPHICS_RIGHT-(x-1)),
|
||||
APPLY_VDEADBAND(y+(size/2)), size, 5, 10, 4, 7, 10, 100, HUD_VSCALE_FLAG_NO_NEGATIVE);
|
||||
if(OsdSettings.AltitudeSource == OSDSETTINGS_ALTITUDESOURCE_BARO)
|
||||
{
|
||||
hud_draw_vertical_scale((int)baro.Altitude, 50, -1, APPLY_HDEADBAND((x+size+1)),
|
||||
APPLY_VDEADBAND(y+(size/2)), size, 10, 20, 4, 7, 10, 500, 0);
|
||||
}
|
||||
else
|
||||
{
|
||||
hud_draw_vertical_scale((int)gpsData.Altitude, 50, -1, APPLY_HDEADBAND((x+size+1)),
|
||||
APPLY_VDEADBAND(y+(size/2)), size, 10, 20, 4, 7, 10, 500, 0);
|
||||
int size = 64;
|
||||
int x = ((GRAPHICS_RIGHT / 2) - (size / 2)), y = (GRAPHICS_BOTTOM - size - 2);
|
||||
draw_artificial_horizon(-attitude.Roll, attitude.Pitch, APPLY_HDEADBAND(x), APPLY_VDEADBAND(y), size);
|
||||
hud_draw_vertical_scale((int) gpsData.Groundspeed, 20, +1, APPLY_HDEADBAND(GRAPHICS_RIGHT-(x-1)), APPLY_VDEADBAND(y+(size/2)), size, 5, 10, 4, 7,
|
||||
10, 100, HUD_VSCALE_FLAG_NO_NEGATIVE);
|
||||
if (OsdSettings.AltitudeSource == OSDSETTINGS_ALTITUDESOURCE_BARO) {
|
||||
hud_draw_vertical_scale((int) baro.Altitude, 50, -1, APPLY_HDEADBAND((x+size+1)), APPLY_VDEADBAND(y+(size/2)), size, 10, 20, 4, 7, 10, 500, 0);
|
||||
} else {
|
||||
hud_draw_vertical_scale((int) gpsData.Altitude, 50, -1, APPLY_HDEADBAND((x+size+1)), APPLY_VDEADBAND(y+(size/2)), size, 10, 20, 4, 7, 10, 500,
|
||||
0);
|
||||
}
|
||||
|
||||
}
|
||||
@ -2396,32 +2283,32 @@ void updateGraphics() {
|
||||
case 5:
|
||||
case 6:
|
||||
{
|
||||
int image=OsdSettings.Screen-4;
|
||||
int image = OsdSettings.Screen - 4;
|
||||
struct splashEntry splash_info;
|
||||
splash_info = splash[image];
|
||||
|
||||
copyimage(APPLY_HDEADBAND(GRAPHICS_RIGHT/2-(splash_info.width)/2), APPLY_VDEADBAND(GRAPHICS_BOTTOM/2-(splash_info.height)/2),image);
|
||||
copyimage(APPLY_HDEADBAND(GRAPHICS_RIGHT/2-(splash_info.width)/2), APPLY_VDEADBAND(GRAPHICS_BOTTOM/2-(splash_info.height)/2), image);
|
||||
}
|
||||
break;
|
||||
default:
|
||||
write_vline_lm( APPLY_HDEADBAND(GRAPHICS_RIGHT/2),APPLY_VDEADBAND(0),APPLY_VDEADBAND(GRAPHICS_BOTTOM),1,1);
|
||||
write_hline_lm( APPLY_HDEADBAND(0),APPLY_HDEADBAND(GRAPHICS_RIGHT),APPLY_VDEADBAND(GRAPHICS_BOTTOM/2),1,1);
|
||||
write_vline_lm(APPLY_HDEADBAND(GRAPHICS_RIGHT/2), APPLY_VDEADBAND(0), APPLY_VDEADBAND(GRAPHICS_BOTTOM), 1, 1);
|
||||
write_hline_lm(APPLY_HDEADBAND(0), APPLY_HDEADBAND(GRAPHICS_RIGHT), APPLY_VDEADBAND(GRAPHICS_BOTTOM/2), 1, 1);
|
||||
break;
|
||||
}
|
||||
|
||||
// Must mask out last half-word because SPI keeps clocking it out otherwise
|
||||
for (uint32_t i = 0; i < 8; i++) {
|
||||
write_vline( draw_buffer_level,GRAPHICS_WIDTH_REAL-i-1,0,GRAPHICS_HEIGHT_REAL-1,0);
|
||||
write_vline( draw_buffer_mask,GRAPHICS_WIDTH_REAL-i-1,0,GRAPHICS_HEIGHT_REAL-1,0);
|
||||
write_vline(draw_buffer_level, GRAPHICS_WIDTH_REAL - i - 1, 0, GRAPHICS_HEIGHT_REAL - 1, 0);
|
||||
write_vline(draw_buffer_mask, GRAPHICS_WIDTH_REAL - i - 1, 0, GRAPHICS_HEIGHT_REAL - 1, 0);
|
||||
}
|
||||
}
|
||||
|
||||
void updateOnceEveryFrame() {
|
||||
void updateOnceEveryFrame()
|
||||
{
|
||||
clearGraphics();
|
||||
updateGraphics();
|
||||
}
|
||||
|
||||
|
||||
// ****************
|
||||
/**
|
||||
* Initialise the gps module
|
||||
@ -2432,8 +2319,8 @@ void updateOnceEveryFrame() {
|
||||
int32_t osdgenStart(void)
|
||||
{
|
||||
// Start gps task
|
||||
vSemaphoreCreateBinary( osdSemaphore);
|
||||
xTaskCreate(osdgenTask, (signed char *)"OSDGEN", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &osdgenTaskHandle);
|
||||
vSemaphoreCreateBinary(osdSemaphore);
|
||||
xTaskCreate(osdgenTask, (signed char *) "OSDGEN", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &osdgenTaskHandle);
|
||||
TaskMonitorAdd(TASKINFO_RUNNING_OSDGEN, osdgenTaskHandle);
|
||||
|
||||
return 0;
|
||||
@ -2462,7 +2349,7 @@ int32_t osdgenInitialize(void)
|
||||
|
||||
return 0;
|
||||
}
|
||||
MODULE_INITCALL(osdgenInitialize, osdgenStart)
|
||||
MODULE_INITCALL( osdgenInitialize, osdgenStart)
|
||||
|
||||
// ****************
|
||||
/**
|
||||
@ -2475,34 +2362,28 @@ static void osdgenTask(void *parameters)
|
||||
// Loop forever
|
||||
//lastSysTime = xTaskGetTickCount();
|
||||
OsdSettingsData OsdSettings;
|
||||
OsdSettingsGet (&OsdSettings);
|
||||
OsdSettingsGet(&OsdSettings);
|
||||
|
||||
PIOS_Servo_Set(0,OsdSettings.White);
|
||||
PIOS_Servo_Set(1,OsdSettings.Black);
|
||||
PIOS_Servo_Set(0, OsdSettings.White);
|
||||
PIOS_Servo_Set(1, OsdSettings.Black);
|
||||
|
||||
// intro
|
||||
for(int i=0; i<63; i++)
|
||||
{
|
||||
if( xSemaphoreTake( osdSemaphore, LONG_TIME ) == pdTRUE )
|
||||
{
|
||||
for (int i = 0; i < 63; i++) {
|
||||
if (xSemaphoreTake(osdSemaphore, LONG_TIME) == pdTRUE) {
|
||||
clearGraphics();
|
||||
introGraphics();
|
||||
}
|
||||
}
|
||||
for(int i=0; i<63; i++)
|
||||
{
|
||||
if( xSemaphoreTake( osdSemaphore, LONG_TIME ) == pdTRUE )
|
||||
{
|
||||
for (int i = 0; i < 63; i++) {
|
||||
if (xSemaphoreTake(osdSemaphore, LONG_TIME) == pdTRUE) {
|
||||
clearGraphics();
|
||||
introGraphics();
|
||||
introText();
|
||||
}
|
||||
}
|
||||
|
||||
while (1)
|
||||
{
|
||||
if( xSemaphoreTake( osdSemaphore, LONG_TIME ) == pdTRUE )
|
||||
{
|
||||
while (1) {
|
||||
if (xSemaphoreTake(osdSemaphore, LONG_TIME) == pdTRUE) {
|
||||
updateOnceEveryFrame();
|
||||
}
|
||||
//xSemaphoreTake(osdSemaphore, portMAX_DELAY);
|
||||
@ -2510,7 +2391,6 @@ static void osdgenTask(void *parameters)
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// ****************
|
||||
|
||||
/**
|
||||
|
||||
@ -75,7 +75,6 @@
|
||||
#include "poilocation.h"
|
||||
#include "accessorydesired.h"
|
||||
|
||||
|
||||
// Private constants
|
||||
#define MAX_QUEUE_SIZE 4
|
||||
#define STACK_SIZE_BYTES 1548
|
||||
@ -88,9 +87,9 @@
|
||||
|
||||
// Private variables
|
||||
static xTaskHandle pathfollowerTaskHandle;
|
||||
static PathDesiredData pathDesired;
|
||||
static PathStatusData pathStatus;
|
||||
static VtolPathFollowerSettingsData vtolpathfollowerSettings;
|
||||
static float poiRadius;
|
||||
|
||||
// Private functions
|
||||
static void vtolPathFollowerTask(void *parameters);
|
||||
@ -113,7 +112,7 @@ int32_t VtolPathFollowerStart()
|
||||
{
|
||||
if (vtolpathfollower_enabled) {
|
||||
// Start main task
|
||||
xTaskCreate(vtolPathFollowerTask, (signed char *)"VtolPathFollower", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle);
|
||||
xTaskCreate(vtolPathFollowerTask, (signed char *) "VtolPathFollower", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &pathfollowerTaskHandle);
|
||||
TaskMonitorAdd(TASKINFO_RUNNING_PATHFOLLOWER, pathfollowerTaskHandle);
|
||||
}
|
||||
|
||||
@ -148,7 +147,7 @@ int32_t VtolPathFollowerInitialize()
|
||||
return 0;
|
||||
}
|
||||
|
||||
MODULE_INITCALL(VtolPathFollowerInitialize, VtolPathFollowerStart)
|
||||
MODULE_INITCALL( VtolPathFollowerInitialize, VtolPathFollowerStart)
|
||||
|
||||
static float northVelIntegral = 0;
|
||||
static float eastVelIntegral = 0;
|
||||
@ -170,11 +169,9 @@ static void vtolPathFollowerTask(void *parameters)
|
||||
portTickType lastUpdateTime;
|
||||
|
||||
VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb);
|
||||
PathDesiredConnectCallback(SettingsUpdatedCb);
|
||||
AccessoryDesiredConnectCallback(accessoryUpdated);
|
||||
|
||||
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
|
||||
PathDesiredGet(&pathDesired);
|
||||
|
||||
// Main task loop
|
||||
lastUpdateTime = xTaskGetTickCount();
|
||||
@ -186,19 +183,13 @@ static void vtolPathFollowerTask(void *parameters)
|
||||
// FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path
|
||||
|
||||
SystemSettingsGet(&systemSettings);
|
||||
if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) &&
|
||||
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) &&
|
||||
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) &&
|
||||
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) &&
|
||||
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) &&
|
||||
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX) &&
|
||||
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) &&
|
||||
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) &&
|
||||
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) &&
|
||||
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP) &&
|
||||
(systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI) )
|
||||
{
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING);
|
||||
if ((systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP)
|
||||
&& (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX)
|
||||
&& (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX)
|
||||
&& (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO)
|
||||
&& (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP)
|
||||
&& (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI)) {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
|
||||
vTaskDelay(1000);
|
||||
continue;
|
||||
}
|
||||
@ -211,24 +202,26 @@ static void vtolPathFollowerTask(void *parameters)
|
||||
|
||||
FlightStatusGet(&flightStatus);
|
||||
PathStatusGet(&pathStatus);
|
||||
PathDesiredData pathDesired;
|
||||
PathDesiredGet(&pathDesired);
|
||||
|
||||
// Check the combinations of flightmode and pathdesired mode
|
||||
switch(flightStatus.FlightMode) {
|
||||
switch (flightStatus.FlightMode) {
|
||||
case FLIGHTSTATUS_FLIGHTMODE_LAND:
|
||||
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
|
||||
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
|
||||
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
|
||||
updateEndpointVelocity();
|
||||
updateVtolDesiredAttitude(false);
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
|
||||
} else {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR);
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
|
||||
}
|
||||
break;
|
||||
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
|
||||
pathStatus.UID = pathDesired.UID;
|
||||
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
|
||||
switch(pathDesired.Mode) {
|
||||
switch (pathDesired.Mode) {
|
||||
// TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly
|
||||
case PATHDESIRED_MODE_FLYENDPOINT:
|
||||
case PATHDESIRED_MODE_FLYVECTOR:
|
||||
@ -236,18 +229,18 @@ static void vtolPathFollowerTask(void *parameters)
|
||||
case PATHDESIRED_MODE_FLYCIRCLELEFT:
|
||||
updatePathVelocity();
|
||||
updateVtolDesiredAttitude(false);
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
|
||||
break;
|
||||
case PATHDESIRED_MODE_FIXEDATTITUDE:
|
||||
updateFixedAttitude(pathDesired.ModeParameters);
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
|
||||
break;
|
||||
case PATHDESIRED_MODE_DISARMALARM:
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_CRITICAL);
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
break;
|
||||
default:
|
||||
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR);
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
|
||||
break;
|
||||
}
|
||||
PathStatusSet(&pathStatus);
|
||||
@ -258,7 +251,7 @@ static void vtolPathFollowerTask(void *parameters)
|
||||
updateVtolDesiredAttitude(true);
|
||||
updatePOIBearing();
|
||||
} else {
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_ERROR);
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
|
||||
}
|
||||
break;
|
||||
default:
|
||||
@ -288,6 +281,12 @@ static void vtolPathFollowerTask(void *parameters)
|
||||
*/
|
||||
static void updatePOIBearing()
|
||||
{
|
||||
const float DEADBAND_HIGH = 0.10;
|
||||
const float DEADBAND_LOW = -0.10;
|
||||
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
|
||||
|
||||
PathDesiredData pathDesired;
|
||||
PathDesiredGet(&pathDesired);
|
||||
PositionActualData positionActual;
|
||||
PositionActualGet(&positionActual);
|
||||
CameraDesiredData cameraDesired;
|
||||
@ -296,41 +295,73 @@ static void updatePOIBearing()
|
||||
StabilizationDesiredGet(&stabDesired);
|
||||
PoiLocationData poi;
|
||||
PoiLocationGet(&poi);
|
||||
//use poi here
|
||||
//HomeLocationData poi;
|
||||
//HomeLocationGet (&poi);
|
||||
|
||||
float dLoc[3];
|
||||
float yaw=0;
|
||||
float elevation=0;
|
||||
float yaw = 0;
|
||||
float elevation = 0;
|
||||
|
||||
dLoc[0]=positionActual.North-poi.North;
|
||||
dLoc[1]=positionActual.East-poi.East;
|
||||
dLoc[2]=positionActual.Down-poi.Down;
|
||||
dLoc[0] = positionActual.North - poi.North;
|
||||
dLoc[1] = positionActual.East - poi.East;
|
||||
dLoc[2] = positionActual.Down - poi.Down;
|
||||
|
||||
if(dLoc[1]<0)
|
||||
yaw=RAD2DEG(atan2f(dLoc[1],dLoc[0]))+180;
|
||||
else
|
||||
yaw=RAD2DEG(atan2f(dLoc[1],dLoc[0]))-180;
|
||||
if (dLoc[1] < 0) {
|
||||
yaw = RAD2DEG(atan2f(dLoc[1],dLoc[0])) + 180;
|
||||
} else {
|
||||
yaw = RAD2DEG(atan2f(dLoc[1],dLoc[0])) - 180;
|
||||
}
|
||||
|
||||
// distance
|
||||
float distance = sqrtf(powf(dLoc[0],2)+powf(dLoc[1],2));
|
||||
float distance = sqrtf(powf(dLoc[0], 2) + powf(dLoc[1], 2));
|
||||
|
||||
//not above
|
||||
if(distance!=0) {
|
||||
//You can feed this into camerastabilization
|
||||
elevation=RAD2DEG(atan2f(dLoc[2],distance));
|
||||
ManualControlCommandData manualControlData;
|
||||
ManualControlCommandGet(&manualControlData);
|
||||
|
||||
float changeRadius = 0;
|
||||
// Move closer or further, radially
|
||||
if (manualControlData.Pitch > DEADBAND_HIGH) {
|
||||
changeRadius = (manualControlData.Pitch - DEADBAND_HIGH) * dT * 100.0f;
|
||||
} else if (manualControlData.Pitch < DEADBAND_LOW) {
|
||||
changeRadius = (manualControlData.Pitch - DEADBAND_LOW) * dT * 100.0f;
|
||||
}
|
||||
stabDesired.Yaw=yaw;
|
||||
|
||||
// move along circular path
|
||||
float pathAngle = 0;
|
||||
if (manualControlData.Roll > DEADBAND_HIGH) {
|
||||
pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f;
|
||||
} else if (manualControlData.Roll < DEADBAND_LOW) {
|
||||
pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f;
|
||||
} else if (manualControlData.Roll >= DEADBAND_LOW && manualControlData.Roll <= DEADBAND_HIGH) {
|
||||
// change radius only when not circling
|
||||
poiRadius = distance + changeRadius;
|
||||
}
|
||||
|
||||
// don't try to move any closer
|
||||
if (poiRadius >= 3.0f || changeRadius > 0) {
|
||||
if (pathAngle != 0 || changeRadius != 0) {
|
||||
pathDesired.End[PATHDESIRED_END_NORTH] = poi.North + (poiRadius * cosf((pathAngle + yaw - 180.0f) * DEG2RAD));
|
||||
pathDesired.End[PATHDESIRED_END_EAST] = poi.East + (poiRadius * sinf((pathAngle + yaw - 180.0f) * DEG2RAD));
|
||||
pathDesired.StartingVelocity = 1;
|
||||
pathDesired.EndingVelocity = 0;
|
||||
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
|
||||
PathDesiredSet(&pathDesired);
|
||||
}
|
||||
}
|
||||
//not above
|
||||
if (distance >= 3.0f) {
|
||||
//You can feed this into camerastabilization
|
||||
elevation = RAD2DEG(atan2f(dLoc[2],distance));
|
||||
|
||||
stabDesired.Yaw = yaw + (pathAngle / 2.0f);
|
||||
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
cameraDesired.Yaw=yaw;
|
||||
cameraDesired.PitchOrServo2=elevation;
|
||||
|
||||
//cameraDesired.Yaw=yaw;
|
||||
//cameraDesired.PitchOrServo2=elevation;
|
||||
|
||||
CameraDesiredSet(&cameraDesired);
|
||||
StabilizationDesiredSet(&stabDesired);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Compute desired velocity from the current position and path
|
||||
*
|
||||
@ -342,10 +373,13 @@ static void updatePathVelocity()
|
||||
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
|
||||
float downCommand;
|
||||
|
||||
PathDesiredData pathDesired;
|
||||
PathDesiredGet(&pathDesired);
|
||||
PositionActualData positionActual;
|
||||
PositionActualGet(&positionActual);
|
||||
|
||||
float cur[3] = {positionActual.North, positionActual.East, positionActual.Down};
|
||||
float cur[3] =
|
||||
{ positionActual.North, positionActual.East, positionActual.Down };
|
||||
struct path_status progress;
|
||||
|
||||
path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode);
|
||||
@ -360,17 +394,16 @@ static void updatePathVelocity()
|
||||
break;
|
||||
case PATHDESIRED_MODE_FLYENDPOINT:
|
||||
case PATHDESIRED_MODE_DRIVEENDPOINT:
|
||||
groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity *
|
||||
bound(progress.fractional_progress,0,1);
|
||||
if(progress.fractional_progress > 1)
|
||||
groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * bound(progress.fractional_progress, 0, 1);
|
||||
if (progress.fractional_progress > 1)
|
||||
groundspeed = 0;
|
||||
break;
|
||||
case PATHDESIRED_MODE_FLYVECTOR:
|
||||
case PATHDESIRED_MODE_DRIVEVECTOR:
|
||||
default:
|
||||
groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
|
||||
bound(progress.fractional_progress,0,1);
|
||||
if(progress.fractional_progress > 1)
|
||||
groundspeed = pathDesired.StartingVelocity
|
||||
+ (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound(progress.fractional_progress, 0, 1);
|
||||
if (progress.fractional_progress > 1)
|
||||
groundspeed = 0;
|
||||
break;
|
||||
}
|
||||
@ -380,28 +413,25 @@ static void updatePathVelocity()
|
||||
velocityDesired.East = progress.path_direction[1] * groundspeed;
|
||||
|
||||
float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP];
|
||||
float correction_velocity[2] = {progress.correction_direction[0] * error_speed,
|
||||
progress.correction_direction[1] * error_speed};
|
||||
float correction_velocity[2] =
|
||||
{ progress.correction_direction[0] * error_speed, progress.correction_direction[1] * error_speed };
|
||||
|
||||
float total_vel = sqrtf(powf(correction_velocity[0],2) + powf(correction_velocity[1],2));
|
||||
float total_vel = sqrtf(powf(correction_velocity[0], 2) + powf(correction_velocity[1], 2));
|
||||
float scale = 1;
|
||||
if(total_vel > vtolpathfollowerSettings.HorizontalVelMax)
|
||||
if (total_vel > vtolpathfollowerSettings.HorizontalVelMax)
|
||||
scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel;
|
||||
|
||||
velocityDesired.North += progress.correction_direction[0] * error_speed * scale;
|
||||
velocityDesired.East += progress.correction_direction[1] * error_speed * scale;
|
||||
|
||||
float altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) *
|
||||
bound(progress.fractional_progress,0,1);
|
||||
float altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * bound(progress.fractional_progress, 0, 1);
|
||||
|
||||
float downError = altitudeSetpoint - positionActual.Down;
|
||||
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KI],
|
||||
-vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT],
|
||||
vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]);
|
||||
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
|
||||
velocityDesired.Down = bound(downCommand,
|
||||
-vtolpathfollowerSettings.VerticalVelMax,
|
||||
vtolpathfollowerSettings.VerticalVelMax);
|
||||
velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
|
||||
|
||||
// update pathstatus
|
||||
pathStatus.error = progress.error;
|
||||
@ -420,6 +450,9 @@ void updateEndpointVelocity()
|
||||
{
|
||||
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
|
||||
|
||||
PathDesiredData pathDesired;
|
||||
PathDesiredGet(&pathDesired);
|
||||
|
||||
PositionActualData positionActual;
|
||||
VelocityDesiredData velocityDesired;
|
||||
|
||||
@ -450,12 +483,12 @@ void updateEndpointVelocity()
|
||||
HomeLocationGet(&homeLocation);
|
||||
float lat = homeLocation.Latitude / 10.0e6f * DEG2RAD;
|
||||
float alt = homeLocation.Altitude;
|
||||
float T[3] = { alt+6.378137E6f,
|
||||
cosf(lat)*(alt+6.378137E6f),
|
||||
-1.0f};
|
||||
float NED[3] = {T[0] * ((gpsPosition.Latitude - homeLocation.Latitude) / 10.0e6f * DEG2RAD),
|
||||
T[1] * ((gpsPosition.Longitude - homeLocation.Longitude) / 10.0e6f * DEG2RAD),
|
||||
T[2] * ((gpsPosition.Altitude + gpsPosition.GeoidSeparation - homeLocation.Altitude))};
|
||||
float T[3] =
|
||||
{ alt + 6.378137E6f, cosf(lat) * (alt + 6.378137E6f), -1.0f };
|
||||
float NED[3] =
|
||||
{ T[0] * ((gpsPosition.Latitude - homeLocation.Latitude) / 10.0e6f * DEG2RAD), T[1]
|
||||
* ((gpsPosition.Longitude - homeLocation.Longitude) / 10.0e6f * DEG2RAD), T[2]
|
||||
* ((gpsPosition.Altitude + gpsPosition.GeoidSeparation - homeLocation.Altitude)) };
|
||||
|
||||
northPos = NED[0];
|
||||
eastPos = NED[1];
|
||||
@ -472,20 +505,18 @@ void updateEndpointVelocity()
|
||||
northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
|
||||
-vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT],
|
||||
vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
|
||||
northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] +
|
||||
northPosIntegral);
|
||||
northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + northPosIntegral);
|
||||
|
||||
eastError = pathDesired.End[PATHDESIRED_END_EAST] - eastPos;
|
||||
eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI],
|
||||
-vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT],
|
||||
vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT]);
|
||||
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] +
|
||||
eastPosIntegral);
|
||||
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP] + eastPosIntegral);
|
||||
|
||||
// Limit the maximum velocity
|
||||
float total_vel = sqrtf(powf(northCommand,2) + powf(eastCommand,2));
|
||||
float total_vel = sqrtf(powf(northCommand, 2) + powf(eastCommand, 2));
|
||||
float scale = 1;
|
||||
if(total_vel > vtolpathfollowerSettings.HorizontalVelMax)
|
||||
if (total_vel > vtolpathfollowerSettings.HorizontalVelMax)
|
||||
scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel;
|
||||
|
||||
velocityDesired.North = northCommand * scale;
|
||||
@ -496,9 +527,7 @@ void updateEndpointVelocity()
|
||||
-vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT],
|
||||
vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_ILIMIT]);
|
||||
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI[VTOLPATHFOLLOWERSETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
|
||||
velocityDesired.Down = bound(downCommand,
|
||||
-vtolpathfollowerSettings.VerticalVelMax,
|
||||
vtolpathfollowerSettings.VerticalVelMax);
|
||||
velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
|
||||
|
||||
VelocityDesiredSet(&velocityDesired);
|
||||
}
|
||||
@ -600,20 +629,18 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
|
||||
northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI],
|
||||
-vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT],
|
||||
vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
|
||||
northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] +
|
||||
northVelIntegral -
|
||||
nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] +
|
||||
velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward);
|
||||
northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + northVelIntegral
|
||||
- nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD]
|
||||
+ velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward);
|
||||
|
||||
// Compute desired east command
|
||||
eastError = velocityDesired.East - eastVel;
|
||||
eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI],
|
||||
-vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT],
|
||||
vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_ILIMIT]);
|
||||
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] +
|
||||
eastVelIntegral -
|
||||
nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD] +
|
||||
velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward);
|
||||
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP] + eastVelIntegral
|
||||
- nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD]
|
||||
+ velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward);
|
||||
|
||||
// Compute desired down command
|
||||
downError = velocityDesired.Down - downVel;
|
||||
@ -622,22 +649,19 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
|
||||
downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KI],
|
||||
-vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT],
|
||||
vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_ILIMIT]);
|
||||
downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP] +
|
||||
downVelIntegral -
|
||||
nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD]);
|
||||
downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KP] + downVelIntegral
|
||||
- nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD]);
|
||||
|
||||
stabDesired.Throttle = bound(downCommand + throttleOffset, 0, 1);
|
||||
|
||||
// Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the
|
||||
// craft should move similarly for 5 deg roll versus 5 deg pitch
|
||||
stabDesired.Pitch = bound(-northCommand * cosf(attitudeActual.Yaw * M_PI / 180) +
|
||||
-eastCommand * sinf(attitudeActual.Yaw * M_PI / 180),
|
||||
stabDesired.Pitch = bound(-northCommand * cosf(attitudeActual.Yaw * M_PI / 180) + -eastCommand * sinf(attitudeActual.Yaw * M_PI / 180),
|
||||
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
|
||||
stabDesired.Roll = bound(-northCommand * sinf(attitudeActual.Yaw * M_PI / 180) +
|
||||
eastCommand * cosf(attitudeActual.Yaw * M_PI / 180),
|
||||
stabDesired.Roll = bound(-northCommand * sinf(attitudeActual.Yaw * M_PI / 180) + eastCommand * cosf(attitudeActual.Yaw * M_PI / 180),
|
||||
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
|
||||
|
||||
if(vtolpathfollowerSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) {
|
||||
if (vtolpathfollowerSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) {
|
||||
// For now override throttle with manual control. Disable at your risk, quad goes to China.
|
||||
ManualControlCommandData manualControl;
|
||||
ManualControlCommandGet(&manualControl);
|
||||
@ -646,7 +670,7 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
|
||||
|
||||
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
if(yaw_attitude) {
|
||||
if (yaw_attitude) {
|
||||
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
} else {
|
||||
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
|
||||
@ -675,15 +699,15 @@ static void updateNedAccel()
|
||||
//rotate avg accels into earth frame and store it
|
||||
AttitudeActualData attitudeActual;
|
||||
AttitudeActualGet(&attitudeActual);
|
||||
q[0]=attitudeActual.q1;
|
||||
q[1]=attitudeActual.q2;
|
||||
q[2]=attitudeActual.q3;
|
||||
q[3]=attitudeActual.q4;
|
||||
q[0] = attitudeActual.q1;
|
||||
q[1] = attitudeActual.q2;
|
||||
q[2] = attitudeActual.q3;
|
||||
q[3] = attitudeActual.q4;
|
||||
Quaternion2R(q, Rbe);
|
||||
for (uint8_t i=0; i<3; i++){
|
||||
accel_ned[i]=0;
|
||||
for (uint8_t j=0; j<3; j++)
|
||||
accel_ned[i] += Rbe[j][i]*accel[j];
|
||||
for (uint8_t i = 0; i < 3; i++) {
|
||||
accel_ned[i] = 0;
|
||||
for (uint8_t j = 0; j < 3; j++)
|
||||
accel_ned[i] += Rbe[j][i] * accel[j];
|
||||
}
|
||||
accel_ned[2] += 9.81f;
|
||||
|
||||
@ -711,7 +735,6 @@ static float bound(float val, float min, float max)
|
||||
static void SettingsUpdatedCb(UAVObjEvent * ev)
|
||||
{
|
||||
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);
|
||||
PathDesiredGet(&pathDesired);
|
||||
}
|
||||
|
||||
static void accessoryUpdated(UAVObjEvent* ev)
|
||||
@ -719,20 +742,20 @@ static void accessoryUpdated(UAVObjEvent* ev)
|
||||
if (ev->obj != AccessoryDesiredHandle())
|
||||
return;
|
||||
|
||||
PositionActualData positionActual;
|
||||
PositionActualGet(&positionActual);
|
||||
AccessoryDesiredData accessory;
|
||||
PoiLearnSettingsData poiLearn;
|
||||
PoiLearnSettingsGet(&poiLearn);
|
||||
PoiLocationData poi;
|
||||
PoiLocationGet(&poi);
|
||||
|
||||
if (poiLearn.Input != POILEARNSETTINGS_INPUT_NONE) {
|
||||
if (AccessoryDesiredInstGet(poiLearn.Input - POILEARNSETTINGS_INPUT_ACCESSORY0, &accessory) == 0) {
|
||||
if(accessory.AccessoryVal<-0.5f)
|
||||
{
|
||||
poi.North=positionActual.North;
|
||||
poi.East=positionActual.East;
|
||||
poi.Down=positionActual.Down;
|
||||
if (accessory.AccessoryVal < -0.5f) {
|
||||
PositionActualData positionActual;
|
||||
PositionActualGet(&positionActual);
|
||||
PoiLocationData poi;
|
||||
PoiLocationGet(&poi);
|
||||
poi.North = positionActual.North;
|
||||
poi.East = positionActual.East;
|
||||
poi.Down = positionActual.Down;
|
||||
PoiLocationSet(&poi);
|
||||
}
|
||||
}
|
||||
|
||||
@ -49,10 +49,11 @@ bool AeroSimRCSimulator::setupProcess()
|
||||
void AeroSimRCSimulator::setupUdpPorts(const QString &host, int inPort, int outPort)
|
||||
{
|
||||
Q_UNUSED(outPort)
|
||||
if (inSocket->bind(QHostAddress(host), inPort))
|
||||
if (inSocket->bind(QHostAddress(host), inPort)) {
|
||||
emit processOutput("Successfully bound to address " + host + ", port " + QString::number(inPort) + "\n");
|
||||
else
|
||||
} else {
|
||||
emit processOutput("Cannot bind to address " + host + ", port " + QString::number(inPort) + "\n");
|
||||
}
|
||||
}
|
||||
|
||||
void AeroSimRCSimulator::transmitUpdate()
|
||||
@ -79,20 +80,16 @@ void AeroSimRCSimulator::transmitUpdate()
|
||||
float yaw = -1;
|
||||
float throttle = -1;
|
||||
|
||||
if(flightStatusData.FlightMode == FlightStatus::FLIGHTMODE_MANUAL)
|
||||
{
|
||||
if (flightStatusData.FlightMode == FlightStatus::FLIGHTMODE_MANUAL) {
|
||||
// Read joystick input
|
||||
if(flightStatusData.Armed == FlightStatus::ARMED_ARMED)
|
||||
{
|
||||
if (flightStatusData.Armed == FlightStatus::ARMED_ARMED) {
|
||||
// Note: Pitch sign is reversed in FG ?
|
||||
roll = manCtrlData.Roll;
|
||||
pitch = -manCtrlData.Pitch;
|
||||
yaw = manCtrlData.Yaw;
|
||||
throttle = manCtrlData.Throttle;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
} else {
|
||||
// Read ActuatorDesired from autopilot
|
||||
actData = actDesired->getData();
|
||||
|
||||
@ -101,12 +98,13 @@ void AeroSimRCSimulator::transmitUpdate()
|
||||
yaw = actData.Yaw;
|
||||
throttle = (actData.Throttle*2.0)-1.0;
|
||||
}
|
||||
channels[0]=roll;
|
||||
channels[1]=pitch;
|
||||
if(throttle<-1)
|
||||
throttle=-1;
|
||||
channels[2]=throttle;
|
||||
channels[3]=yaw;
|
||||
channels[0] = roll;
|
||||
channels[1] = pitch;
|
||||
if(throttle < -1) {
|
||||
throttle = -1;
|
||||
}
|
||||
channels[2] = throttle;
|
||||
channels[3] = yaw;
|
||||
|
||||
// read flight status
|
||||
quint8 armed;
|
||||
@ -120,8 +118,9 @@ void AeroSimRCSimulator::transmitUpdate()
|
||||
QDataStream stream(&data, QIODevice::WriteOnly);
|
||||
stream.setFloatingPointPrecision(QDataStream::SinglePrecision);
|
||||
stream << quint32(0x52434D44); // magic header, "RCMD"
|
||||
for (int i = 0; i < 10; ++i)
|
||||
for (int i = 0; i < 10; ++i) {
|
||||
stream << channels[i]; // channels
|
||||
}
|
||||
stream << armed << mode; // flight status
|
||||
stream << udpCounterASrecv; // packet counter
|
||||
|
||||
@ -193,10 +192,10 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data)
|
||||
memset(&out, 0, sizeof(Output2Hardware));
|
||||
|
||||
|
||||
out.delT=delT;
|
||||
out.delT = delT;
|
||||
|
||||
/*************************************************************************************/
|
||||
for (int i=0; i< AEROSIM_RCCHANNEL_NUMELEM; i++){
|
||||
for (int i=0; i< AEROSIM_RCCHANNEL_NUMELEM; i++) {
|
||||
out.rc_channel[i]=ch[i]; //Elements in rc_channel are between -1 and 1
|
||||
}
|
||||
|
||||
@ -245,22 +244,20 @@ void AeroSimRCSimulator::processUpdate(const QByteArray &data)
|
||||
out.groundspeed = qSqrt(velX * velX + velY * velY);
|
||||
|
||||
/**********************************************************************************************/
|
||||
out.dstN = posY * 1;
|
||||
out.dstE = posX * 1;
|
||||
out.dstD = posZ * -1;
|
||||
out.dstN = posY;
|
||||
out.dstE = posX;
|
||||
out.dstD = -posZ;
|
||||
|
||||
out.velNorth = velY * 1;
|
||||
out.velEast = velX * 1;
|
||||
out.velDown = velZ * -1;
|
||||
out.velNorth = velY;
|
||||
out.velEast = velX;
|
||||
out.velDown = -velZ;
|
||||
|
||||
out.voltage = volt;
|
||||
out.current = curr;
|
||||
out.consumption = cons*1000.0;
|
||||
|
||||
out.consumption = cons * 1000.0;
|
||||
|
||||
updateUAVOs(out);
|
||||
|
||||
|
||||
#ifdef DBG_TIMERS
|
||||
static int cntRX = 0;
|
||||
if (cntRX >= 100) {
|
||||
|
||||
@ -130,7 +130,7 @@ endif
|
||||
ifeq ($(DEBUG), YES)
|
||||
CFLAGS += -DDEBUG
|
||||
else
|
||||
CFLAGS += -fdata-sections -ffunction-sections
|
||||
CFLAGS += -ffunction-sections #-fdata-sections
|
||||
endif
|
||||
|
||||
# Compiler flags to generate dependency files
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user